combine compass and velocity into topic DroneStatus

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<mavros_msgs::Waypoint>(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 &degree){
+    msg.param1 = degree->data;
+    
+}

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);

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();