From 9e32d5168d24e21b4044fab64dbef0e9df22f03a Mon Sep 17 00:00:00 2001 From: Xuan0319 Date: Wed, 21 Dec 2022 21:54:16 +0800 Subject: [PATCH] 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();