From 965d0d33c8287270b098881b030fa9dbc22b8424 Mon Sep 17 00:00:00 2001 From: RangeOfGlitching Date: Wed, 14 Dec 2022 03:01:26 +0800 Subject: [PATCH 1/8] work fine with ros --- class_model/src/main.cpp | 2 +- class_model/src/mqttPubMain.py | 39 +++++++++------- class_model/src/mqttSubMain.py | 44 ++++++++++--------- class_model/src/utils/mqttConfig.yml | 6 +-- .../utils/protoJson_mqtt_pub_data_to_ros.py | 19 +++++--- .../utils/protoJson_mqtt_sub_data_from_ros.py | 2 +- 6 files changed, 67 insertions(+), 45 deletions(-) mode change 100644 => 100755 class_model/src/mqttPubMain.py mode change 100644 => 100755 class_model/src/mqttSubMain.py diff --git a/class_model/src/main.cpp b/class_model/src/main.cpp index 02f6373..a38094e 100755 --- a/class_model/src/main.cpp +++ b/class_model/src/main.cpp @@ -36,7 +36,7 @@ int main(int argc, char **argv) { return 0; } -////////////////////////////////////////////////////// + /* ThreadGPSClass gps_object; diff --git a/class_model/src/mqttPubMain.py b/class_model/src/mqttPubMain.py old mode 100644 new mode 100755 index c2e2638..eaeafd4 --- a/class_model/src/mqttPubMain.py +++ b/class_model/src/mqttPubMain.py @@ -18,7 +18,7 @@ from sensor_msgs.msg import NavSatFix from sensor_msgs.msg import Imu from sensor_msgs.msg import Range from geometry_msgs.msg import Vector3 - +''' # test proto import random @@ -35,11 +35,13 @@ class fake_hdg(): class state(): def __init__(self): self.mode = "test" +''' def init_dataFormat(cfg): + ros_namespace="/drone1" if cfg.msg_format == "Proto": utils.Proto_msg_from_ros.client = client utils.Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message() @@ -47,11 +49,15 @@ def init_dataFormat(cfg): utils.Proto_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS + rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Proto_msg_from_ros.callBack_gps) + rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Proto_msg_from_ros.callBack_compass_hdg) elif cfg.msg_format == "Json": utils.Json_msg_from_ros.client = client utils.Json_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt utils.Json_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt utils.Json_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS + rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps) + rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg) else: print("msg_format not found") @@ -62,16 +68,16 @@ def on_connect(self, userdata, flags, rc): print("Connected with result code " + str(rc)) def on_publish(self, userdata, mid): - pass - + print("pub ok") if __name__ == '__main__': # Read Config - cfg = utils.MQTT_ROS_Config("utils/mqttConfig.yml") + FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig.yml") + cfg = utils.MQTT_ROS_Config(FilePath) client = utils.MQTTClient(cfg.MQTTClientNamePub) - + print(cfg) # Setting Config - init_dataFormat(cfg) + # Mqtt client.on_connect = on_connect @@ -81,12 +87,23 @@ if __name__ == '__main__': client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain) client.loop_start() + # Ros + rosClient = cfg.ROSClientNamePub + rospy.init_node(rosClient) + init_dataFormat(cfg) + + + + rospy.spin() + +''' #test proto gps = fakeGps() hdg = fake_hdg() state = state() while True: #test proto + gps.latitude = random.uniform(0, 10) gps.longitude = random.uniform(0, 10) gps.altitude = random.uniform(0, 10) @@ -100,12 +117,4 @@ if __name__ == '__main__': # utils.Json_msg_from_ros.callBack_compass_hdg(hdg) # utils.Json_msg_from_ros.callBack_state(state) # time.sleep(1) - - # Ros - # rosClient = cfg.ROSClientNamePub - # rospy.init_node(rosClient) - # subscriber = rospy.Subscriber('/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps) - # subscriber = rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg) - - - # rospy.spin() +''' diff --git a/class_model/src/mqttSubMain.py b/class_model/src/mqttSubMain.py old mode 100644 new mode 100755 index 2dfdec5..ede39a2 --- a/class_model/src/mqttSubMain.py +++ b/class_model/src/mqttSubMain.py @@ -12,27 +12,30 @@ from std_msgs.msg import String def init_dataFormat(cfg): if cfg.msg_format == "Proto": - # utils.Proto_msg_to_ros.rate = rospy.Rate(10) - # utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10) - # utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10) + utils.Proto_msg_to_ros.rate = rospy.Rate(10) + utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10) + utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10) utils.Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message() utils.Proto_msg_to_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message() + + client.on_message = utils.Proto_msg_to_ros.on_message utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt elif cfg.msg_format == "Json": - utils.Proto_msg_to_ros.rate = rospy.Rate(10) - utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10) - utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10) - - utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt - utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt + utils.Json_msg_to_ros.rate = rospy.Rate(10) + utils.Json_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10) + utils.Json_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10) + + client.on_message = utils.Json_msg_to_ros.on_message + utils.Json_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt + utils.Json_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt else: print("msg_format not found") -def on_message(client, userdata, msg): - utils.Proto_msg_to_ros.ros_pub(msg) +# def on_message(client, userdata, msg): +# utils.Json_msg_to_ros.ros_pub(msg) def on_connect(self, userdata, flags, rc): print("Connected with result code " + str(rc)) @@ -45,22 +48,23 @@ def on_publish(self, userdata, mid): if __name__ == '__main__': # Read Config - cfg = utils.MQTT_ROS_Config("utils/mqttConfig.yml") - + FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig.yml") + # Read Config + cfg = utils.MQTT_ROS_Config(FilePath) + print(cfg) # Mqtt - init_dataFormat(cfg) client = utils.MQTTClient(cfg.MQTTClientNameSub) client.on_connect = on_connect - client.on_message = on_message + client.on_message = utils.Json_msg_to_ros.on_message client.on_publish = on_publish client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) - client.loop_forever() + # Ros - # rospy.init_node(cfg.ROSClientNameSub) + rospy.init_node(cfg.ROSClientNameSub) # initialize - # init_dataFormat(cfg) - + init_dataFormat(cfg) - # rospy.spin() \ No newline at end of file + client.loop_start() + rospy.spin() \ No newline at end of file diff --git a/class_model/src/utils/mqttConfig.yml b/class_model/src/utils/mqttConfig.yml index f30adcf..86cc5d3 100644 --- a/class_model/src/utils/mqttConfig.yml +++ b/class_model/src/utils/mqttConfig.yml @@ -1,8 +1,8 @@ MQTT_ROS: - msg_format: Proto + msg_format: Json MQTTClientNamePub: Drone650Pub MQTTClientNameSub: Drone650Sub - host: 192.168.50.117 + host: 192.168.50.27 port: 1883 keepalive: 60 willTopic: CheckDoneConnect @@ -18,4 +18,4 @@ MQTT_ROS: ROSClientNamePub: Drone650Pub ROSClientNameSub: Drone650Sub ROStopicName_Flight_Information: Flight_Information_reciver - ROStopicName_Fly_Formation: Fly_Formation_reciver \ No newline at end of file + ROStopicName_Fly_Formation: Fly_Formation_reciver diff --git a/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py b/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py index 2eb6cc4..8b6c210 100644 --- a/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py +++ b/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py @@ -27,16 +27,20 @@ class Proto_msg_to_ros: proto_msg = cls.flight_information_msg.FromString(dataProto.payload) protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True) print(protoTOJson_msg) - # cls.publisher_flight_information.publish(protoTOJson_msg) - # cls.rate.sleep() + cls.publisher_Flight_Information.publish(protoTOJson_msg) + cls.rate.sleep() elif dataProto.topic == cls.Fly_Formation_topicToMqtt: proto_msg = cls.fly_formation_msg.FromString(dataProto.payload) protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True) - cls.publisher_Formation.publish(protoTOJson_msg) + cls.publisher_Fly_Formation.publish(protoTOJson_msg) cls.rate.sleep() else: print("topic not found") + + @staticmethod + def on_message(client, userdata, msg): + Proto_msg_to_ros.ros_pub(msg) class Json_msg_to_ros: @@ -51,10 +55,15 @@ class Json_msg_to_ros: @classmethod def ros_pub(cls, dataJson): if dataJson.topic == cls.Flight_Information_topicToMqtt: - cls.publisher_flight_information.publish(dataJson.payload.decode('utf-8')) + cls.publisher_Flight_Information.publish(dataJson.payload.decode('utf-8')) cls.rate.sleep() elif dataJson.topic == cls.Fly_Formation_topicToMqtt: cls.publisher_Fly_Formation.publish(dataJson.payload.decode('utf-8')) cls.rate.sleep() else: - print("topic not found") \ No newline at end of file + print("topic not found") + + + @staticmethod + def on_message(client, userdata, msg): + Json_msg_to_ros.ros_pub(msg) \ No newline at end of file diff --git a/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py b/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py index 1661f0d..bde5878 100644 --- a/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py +++ b/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py @@ -18,7 +18,7 @@ class Proto_msg_from_ros: cls.flight_information_msg.gps.LON = GPS.longitude cls.flight_information_msg.gps.ALT = GPS.altitude # print ('lat:'+lat+'\n'+'lon:'+lon+'\n') - # print(flight_information_msg) + # print(cls.flight_information_msg) @classmethod def callBack_compass_hdg(cls, Compass): From ed54508e708da61eab4ba8acdf32380454446858 Mon Sep 17 00:00:00 2001 From: Xuan0319 Date: Wed, 14 Dec 2022 03:06:53 +0800 Subject: [PATCH 2/8] ignore remove by huang --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 1b54075..6379c1a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,6 @@ iq_gnc/ ROS_controll/ launch/ +__pycache__/ +*pyc +.todo From 5caac7cbc2c936a3a1eafda80ae877fce1f6bd9e Mon Sep 17 00:00:00 2001 From: Xuan0319 Date: Tue, 20 Dec 2022 21:38:11 +0800 Subject: [PATCH 3/8] debug --- class_model/src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/class_model/src/main.cpp b/class_model/src/main.cpp index a38094e..bb80e52 100755 --- a/class_model/src/main.cpp +++ b/class_model/src/main.cpp @@ -30,7 +30,7 @@ int main(int argc, char **argv) { //json_object.get_command(); //for follower - + } ros::waitForShutdown(); From 506e6e38d80168ea0e4151f3843db1d37287761a Mon Sep 17 00:00:00 2001 From: Xuan0319 Date: Tue, 20 Dec 2022 21:40:01 +0800 Subject: [PATCH 4/8] debug --- class_model/src/requestData.cpp | 40 ++++++--------------------------- 1 file changed, 7 insertions(+), 33 deletions(-) diff --git a/class_model/src/requestData.cpp b/class_model/src/requestData.cpp index d22a2f4..6e3f53d 100755 --- a/class_model/src/requestData.cpp +++ b/class_model/src/requestData.cpp @@ -7,9 +7,9 @@ int command; RequestClass::RequestClass() : node_handle_(""){ - mqtt_data = node_handle_.subscribe("/uav_message", 100, + mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100, &RequestClass::Data_callback, this); - formation_data = node_handle_.subscribe("/formation_message", 10, + formation_data = node_handle_.subscribe("/Fly_Formation_reciver", 10, &RequestClass::Message_callback, this); } @@ -17,13 +17,13 @@ RequestClass::RequestClass() : node_handle_(""){ RequestClass::~RequestClass() { ros::shutdown(); } void RequestClass::Data_callback(const std_msgs::String::ConstPtr &sensor) { - StringToJson(sensor->data); + L_INFO(sensor->data); } void RequestClass::Message_callback(const std_msgs::String::ConstPtr &message) { std::string data = message->data; - jsonToString(data); + // jsonToString(data); } @@ -45,37 +45,11 @@ int RequestClass::get_formation_message(){ void RequestClass::jsonToString(std::string data){ - std::string list[1]={""}; - int j = 0; - - for(int i=0;isizeof(list)){ - break; - } - } - } - } - - command = std::stoi(list[1]); - - // ROS_INFO("command: %s",command); } -void RequestClass::StringToJson(std::string data){ +void RequestClass::L_INFO(std::string data){ - std::cout <<"string: "<< data << std::endl; + // std::cout <<"string: "<< data << std::endl; // document.Parse(data.c_str()); //china's library // leader_position.lat=document["lat"].GetInt(); @@ -102,7 +76,7 @@ void RequestClass::StringToJson(std::string data){ leader_position.alt=j_data["alt"]; heading = j_data["heading"]; - std::cout <<"Json: " < Date: Tue, 20 Dec 2022 21:57:02 +0800 Subject: [PATCH 5/8] debug --- class_model/include/class_model/requestData.h | 3 +-- class_model/src/main.cpp | 4 ++-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/class_model/include/class_model/requestData.h b/class_model/include/class_model/requestData.h index 46e7eb0..26fa369 100755 --- a/class_model/include/class_model/requestData.h +++ b/class_model/include/class_model/requestData.h @@ -27,9 +27,8 @@ private: 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); - void StringToJson(std::string data); + void L_INFO(std::string data); float heading; global_location leader_position; // rapidjson::Document document; diff --git a/class_model/src/main.cpp b/class_model/src/main.cpp index bb80e52..7d72b63 100755 --- a/class_model/src/main.cpp +++ b/class_model/src/main.cpp @@ -15,7 +15,7 @@ int main(int argc, char **argv) { ReceiverClass receiver_object; MissionClass mission_object; CommandClass command_object; - SelectionClass selection_object; + // SelectionClass selection_object; mode_object.set_Mode("GUIDED"); @@ -23,7 +23,7 @@ int main(int argc, char **argv) { control_object.takeoff(4.5); sleep(5); - selection_object.choose_leader(); + // selection_object.choose_leader(); // mission_object.start(); //default hover in goose formation while(ros::ok()){ From f2d16b78a259d53527979aa8239c524f927d6cf0 Mon Sep 17 00:00:00 2001 From: Xuan0319 Date: Tue, 20 Dec 2022 23:18:35 +0800 Subject: [PATCH 6/8] debug --- class_model/src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/class_model/src/main.cpp b/class_model/src/main.cpp index 7d72b63..2d98b8d 100755 --- a/class_model/src/main.cpp +++ b/class_model/src/main.cpp @@ -21,7 +21,7 @@ int main(int argc, char **argv) { mode_object.set_Mode("GUIDED"); control_object.arm(); control_object.takeoff(4.5); - sleep(5); + sleep(2); // selection_object.choose_leader(); // mission_object.start(); //default hover in goose formation From 9e32d5168d24e21b4044fab64dbef0e9df22f03a Mon Sep 17 00:00:00 2001 From: Xuan0319 Date: Wed, 21 Dec 2022 21:54:16 +0800 Subject: [PATCH 7/8] combine compass and velocity into topic DroneStatus --- class_model/CMakeLists.txt | 7 +++- class_model/include/class_model/PubData.h | 39 +++++++++++++++++++++ class_model/include/class_model/formation.h | 3 ++ class_model/include/class_model/velocity.h | 8 +++++ class_model/src/PubData.cpp | 32 +++++++++++++++++ class_model/src/formation.cpp | 19 +++++----- class_model/src/main.cpp | 7 +++- 7 files changed, 104 insertions(+), 11 deletions(-) create mode 100644 class_model/include/class_model/PubData.h create mode 100644 class_model/include/class_model/velocity.h create mode 100644 class_model/src/PubData.cpp diff --git a/class_model/CMakeLists.txt b/class_model/CMakeLists.txt index fa67068..55a9378 100755 --- a/class_model/CMakeLists.txt +++ b/class_model/CMakeLists.txt @@ -255,11 +255,16 @@ target_include_directories(PID_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) target_link_libraries(PID_lib ${catkin_LIBRARIES}) add_dependencies(PID_lib main_generate_messages_cpp) +add_library(PubData_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/PubData.cpp) +target_include_directories(PubData_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) +target_link_libraries(PubData_lib ${catkin_LIBRARIES}) +add_dependencies(PubData_lib main_generate_messages_cpp) + ################## add_library(formation_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/formation.cpp) target_include_directories(formation_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) -target_link_libraries(formation_lib ${catkin_LIBRARIES} sensor_lib mode_lib control_lib command_lib requestData_lib PID_lib) +target_link_libraries(formation_lib ${catkin_LIBRARIES} sensor_lib mode_lib control_lib command_lib requestData_lib PID_lib PubData_lib) add_dependencies(formation_lib main_generate_messages_cpp) add_library(select_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/select.cpp) diff --git a/class_model/include/class_model/PubData.h b/class_model/include/class_model/PubData.h new file mode 100644 index 0000000..900eb5e --- /dev/null +++ b/class_model/include/class_model/PubData.h @@ -0,0 +1,39 @@ +/*Publish Compass & velocity data to topic "DroneStatus"*/ +#ifndef PUBDATA_H +#define PUBDATA_H + +#include +#include +#include +#include +#include "class_model/velocity.h" + +class PubDataClass { +public: + PubDataClass(); + virtual ~PubDataClass(); + void publishData(velocity v); + + + +private: + // ROS NodeHandle + ros::NodeHandle node_handle_; + + //SUBSCRIBE + ros::Subscriber Compass; + + //PUBLISH + ros::Publisher PubData; + + mavros_msgs::Waypoint msg; + float heading; + void Compass_callback(const std_msgs::Float64::ConstPtr °ree); + + + + + +}; + +#endif // PUBDATA_H \ No newline at end of file diff --git a/class_model/include/class_model/formation.h b/class_model/include/class_model/formation.h index 2d8d32e..dd1beea 100755 --- a/class_model/include/class_model/formation.h +++ b/class_model/include/class_model/formation.h @@ -10,6 +10,7 @@ #include "class_model/command.h" #include "class_model/requestData.h" #include "class_model/PID.h" +#include "class_model/PubData.h" #include @@ -25,6 +26,7 @@ public: ControlClass control_object; CommandClass command_object; RequestClass request_object; + PubDataClass PubData_object; PIDClass PID_x; PIDClass PID_y; @@ -48,6 +50,7 @@ private: global_location follower_location; global_location leader_location; global_location current_location; + velocity drone_speed; int flag = 0,heading_status = 0; float pre_alt,cur_alt; float leader_pid[3] = {0.5 , 0.000000000001 ,0.5}; diff --git a/class_model/include/class_model/velocity.h b/class_model/include/class_model/velocity.h new file mode 100644 index 0000000..108c4fa --- /dev/null +++ b/class_model/include/class_model/velocity.h @@ -0,0 +1,8 @@ +#ifndef VELOCITY_H +#define VELOCITY_H + +typedef struct velocity{ + float x,y; +}velocity; + +#endif // VELOCITY_H \ No newline at end of file diff --git a/class_model/src/PubData.cpp b/class_model/src/PubData.cpp new file mode 100644 index 0000000..994516e --- /dev/null +++ b/class_model/src/PubData.cpp @@ -0,0 +1,32 @@ +#include "class_model/PubData.h" + +PubDataClass::PubDataClass() : node_handle_("~"){ + + std::string ros_namespace; + if (!node_handle_.hasParam("namespace")) + { + }else{ + node_handle_.getParam("namespace", ros_namespace); + } + + PubData=node_handle_.advertise(ros_namespace+"/DroneStatus",100); + Compass=node_handle_.subscribe(ros_namespace+"/mavros/global_position/compass_hdg", 10, + &PubDataClass::Compass_callback, this); + +} + +PubDataClass::~PubDataClass() { ros::shutdown(); } + +void PubDataClass::publishData(velocity v){ + + msg.param2 = v.x; + msg.param3 = v.y; + PubData.publish(msg); + ROS_INFO("pub drone status"); + +} + +void PubDataClass::Compass_callback(const std_msgs::Float64::ConstPtr °ree){ + msg.param1 = degree->data; + +} \ No newline at end of file diff --git a/class_model/src/formation.cpp b/class_model/src/formation.cpp index ab213f7..21a6144 100755 --- a/class_model/src/formation.cpp +++ b/class_model/src/formation.cpp @@ -578,25 +578,26 @@ void FormationClass::go2target(float x,float y){ face2target(target_heading); } - float error_x = PID_x.update(current_location.lon*1e5 , target_location.lon ,leader_pid); //leader_pid defined in header file - float error_y = PID_y.update(current_location.lat*1e5 , target_location.lat ,leader_pid); + drone_speed.x = PID_x.update(current_location.lon*1e5 , target_location.lon ,leader_pid); //leader_pid defined in header file + drone_speed.y = PID_y.update(current_location.lat*1e5 , target_location.lat ,leader_pid); // 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 (error_x < 0.3 && error_x > -0.3){ //ignore small errors - error_x = 0; + if (drone_speed.x < 0.3 && drone_speed.x > -0.3){ //ignore small errors + drone_speed.x = 0; } - if (error_y < 0.3 && error_y > -0.3){ - error_y = 0; + if (drone_speed.y < 0.3 && drone_speed.y > -0.3){ + drone_speed.y = 0; } - if (error_x == 0 && error_y == 0){ + if (drone_speed.x == 0 && drone_speed.y == 0){ flag = 1; } - command_object.set_velocity(error_x ,error_y ,0,0,0.1); - + PubData_object.publishData(drone_speed); + 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("heading:%f,target_heading:%f",heading,target_heading); diff --git a/class_model/src/main.cpp b/class_model/src/main.cpp index 2d98b8d..02dfc1b 100755 --- a/class_model/src/main.cpp +++ b/class_model/src/main.cpp @@ -1,5 +1,6 @@ #include "class_model/mission.h" #include "class_model/choose_leader.h" +#include "class_model/PubData.h" int main(int argc, char **argv) { @@ -15,6 +16,9 @@ int main(int argc, char **argv) { ReceiverClass receiver_object; MissionClass mission_object; CommandClass command_object; + SelectClass select_formation; + PubDataClass PubData_object; + // SelectionClass selection_object; @@ -28,8 +32,9 @@ int main(int argc, char **argv) { while(ros::ok()){ - //json_object.get_command(); //for follower + //json_object.get_command(); //for follower + select_formation.goose_formation(); } ros::waitForShutdown(); From 4bd113c30c3b3b0a2b6e56e39dcc05e60dd3d832 Mon Sep 17 00:00:00 2001 From: Xuan0319 Date: Thu, 22 Dec 2022 20:03:52 +0800 Subject: [PATCH 8/8] compass&velocity topic test completed --- class_model/src/test_pub.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100755 class_model/src/test_pub.py diff --git a/class_model/src/test_pub.py b/class_model/src/test_pub.py new file mode 100755 index 0000000..db4d8f7 --- /dev/null +++ b/class_model/src/test_pub.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python3 +#coding:utf-8 +import rospy +from mavros_msgs.msg import Waypoint + +def callBack(data): + print (data.param1) + +if __name__ == '__main__': + nodeName = 'subscriber_py' + rospy.init_node(nodeName) + topicName = '/DroneStatus' + subscriber = rospy.Subscriber(topicName,Waypoint,callBack) #從topic獲取string再呼叫callback + rospy.spin() \ No newline at end of file