combine compass and velocity into topic DroneStatus

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)

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 <ros/ros.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <std_msgs/Float64.h>
+#include <mavros_msgs/Waypoint.h>
+#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 &degree);    
+
+
+
+    
+
+};
+
+#endif // PUBDATA_H

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 <std_msgs/String.h>
 
 
@@ -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};

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

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