first commit

class_model/include/class_model/PID.h

@@ -0,0 +1,27 @@
+/*PID Controlier*/
+#ifndef PID_H
+#define PID_H
+
+#include <ros/ros.h>
+#include "ctime"
+
+class PIDClass {
+public:
+//   PIDClass();
+//   virtual ~PIDClass();
+  float update(float current ,float target ,float pidvals[3]);
+  float update1(float current ,float target );
+private:
+  // ROS NodeHandle
+  ros::NodeHandle node_handle_;
+
+  float pidval1[3] = {0.5 , 0.000000000001 , 0.5};
+  float limit[2] = {2 , -2};
+  float targetvals,pError,I;
+  clock_t pTime = 0,current_time;
+
+
+
+};
+
+#endif // PID_H

class_model/include/class_model/command.h

@@ -0,0 +1,45 @@
+/*Drone Control Command*/
+#ifndef COMMAND_H
+#define COMMAND_H
+
+#include <ros/ros.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <mavros_msgs/GlobalPositionTarget.h>
+#include <class_model/sensor.h>
+
+class CommandClass {
+public:
+    CommandClass();
+    virtual ~CommandClass();
+    void set_global_position(float lat,float lon,float alt);
+    void set_velocity(float x,float y,float alt,float yaw,float second);
+    void fix_velocity(float x,float y,float alt,float yaw,float second);
+    void velocity_init();
+    void set_target_position(float x,float y);
+    ThreadGPSClass gps_object;
+
+private:
+    // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+
+    geometry_msgs::TwistStamped msg;
+    mavros_msgs::GlobalPositionTarget goal_position;
+    global_location current_location;
+    global_location target_location;
+    global_location origin_location;
+    global_location pre_location;
+    float errorX,errorY,lon_speed,lat_speed;
+
+    int fix_position(global_location pre_location,float x,float y,float s);
+    float clip(float speed,float max_speed,float min_speed);
+    //SERVICE
+
+    //SUBSCRIBE
+
+    //PUBLISHER
+    ros::Publisher velocity_command;
+    ros::Publisher gps_command; 
+
+};
+
+#endif // COMMAND_H

class_model/include/class_model/control.h

@@ -0,0 +1,34 @@
+#ifndef CONTROL_H
+#define CONTROL_H
+
+#include <ros/ros.h>
+#include <mavros_msgs/CommandTOL.h>
+#include <mavros_msgs/CommandBool.h>
+#include <mavros_msgs/State.h>
+
+class ControlClass {
+public:
+    ControlClass();
+    virtual ~ControlClass();
+    int arm();
+    int takeoff(float takeoff_alt);
+
+private:
+    // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+
+    mavros_msgs::State current_state_g;
+
+    //SERVICE
+    ros::ServiceClient arming_client;
+    ros::ServiceClient takeoff_client;
+
+    //SUBSCRIBE
+    ros::Subscriber state_sub;
+
+    void state_cb(const mavros_msgs::State::ConstPtr& msg);
+    
+
+};
+
+#endif // CONTROL_H

class_model/include/class_model/convert_degree.h

@@ -0,0 +1,41 @@
+float ConvertDeg(float x ,float y){
+    float degree;
+
+    if(x>0){
+        degree = 90-(atan(y/x)*180/3.14);
+    }
+    if(x<0 && y>0){
+        degree = atan(y/x)*180/3.14 + 360;
+    }
+    if(x<0 && y<0){
+        degree = (atan(y/x)*180/3.14) + 180;
+    }
+    if(x==0 && y>=0){
+        degree = 0;
+    } 
+    if(x==0 && y<0){
+        degree = 180;
+    }if(x>0 && y==0){
+        degree = 90;
+    }if(x<0 && y==0){
+        degree = 270;
+    }
+
+    return degree;
+}
+
+float check_direction(float error_degree){
+    
+    int direction;
+    if(error_degree <= 180 && error_degree >= 0){              //check yaw direction
+        direction = -1;
+    }else if(error_degree < -180){
+        direction = -1;
+    }else if(error_degree >= -180 && error_degree < 0){
+        direction = 1;
+    }else if(error_degree > 180){
+        direction = 1;
+    }
+
+    return direction;
+}

class_model/include/class_model/follower.h

@@ -0,0 +1,36 @@
+/*Follow the leader in a fixed formation */
+#ifndef FOLLOWER_H
+#define FOLLOWER_H
+
+#include <ros/ros.h>
+#include "class_model/sensor.h"
+#include "class_model/mode.h"
+#include "class_model/control.h"
+#include "class_model/command.h"
+#include "class_model/requestData.h"
+
+
+class FollowerClass {
+public:
+    FollowerClass();
+    virtual ~FollowerClass();
+        //ClassObject
+    ThreadGPSClass GPS_object;
+    ModeClass mode_object;
+    ControlClass control_object;
+    CommandClass command_object;
+    RequestClass request_object;
+    void follower();
+private:
+    // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+
+    global_location target_location;
+    global_location follower_location;
+    global_location leader_location;
+
+    void calculate_position(float k,float theta);
+
+};
+
+#endif // FOLLOWER_H

class_model/include/class_model/formation.h

@@ -0,0 +1,68 @@
+/*Follow the leader in a fixed formation */
+#ifndef FORMATION_H
+#define FORMATION_H
+
+#include <ros/ros.h>
+#include "class_model/sensor.h"
+#include "class_model/mode.h"
+#include "class_model/receiver.h"
+#include "class_model/control.h"
+#include "class_model/command.h"
+#include "class_model/requestData.h"
+#include "class_model/PID.h"
+#include <std_msgs/String.h>
+
+
+
+class FormationClass {
+public:
+    FormationClass();
+    virtual ~FormationClass();
+        //ClassObject
+    ThreadGPSClass GPS_object;
+    ModeClass mode_object;
+    ReceiverClass receiver_object;
+    ControlClass control_object;
+    CommandClass command_object;
+    RequestClass request_object;
+    PIDClass PID_x;
+    PIDClass PID_y;
+    
+    void leader();
+    void leader1(float x=0.0,float y=0.0);
+    void follower1(int type);
+    void follower2(int type);
+    void follower3(int type);
+    void follower4(int type);
+    void follower5(int type);
+    void sph_follower1(int type);
+    void sph_follower2(int type);
+    void sph_follower3(int type);
+    void sph_follower4(int type);
+    void face2target(float target_heading);
+private:
+    // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+
+    global_location target_location;
+    global_location follower_location;
+    global_location leader_location;
+    global_location current_location;
+    int flag = 0,heading_status = 0;
+    float pre_alt,cur_alt;
+    float leader_pid[3] = {0.5 , 0.000000000001 ,0.5};
+    float follower_pid[3] = {1 , 0.00000000001 ,0.5};
+
+    void calculate_position(float k,float theta,int direction=0);
+    void spherical_coordinate(float k,float theta,float psi);
+    void go2target(float x,float y);
+
+    //PUBLISHER
+    ros::Publisher next_command;
+
+      // SUBSCRIBE
+    ros::Subscriber command_break;
+
+};
+
+#endif // FORMATION_H

class_model/include/class_model/getParam.h

@@ -0,0 +1,28 @@
+/*Get Drone's Parameter*/
+#ifndef GETPARAM_H
+#define GETPARAM_H
+
+#include <ros/ros.h>
+
+class ParamClass {
+public:
+    ParamClass();
+    virtual ~ParamClass();
+    int getID();
+
+private:
+    // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+
+    //SERVICE
+
+
+    //SUBSCRIBE
+
+
+
+    
+
+};
+
+#endif // GETPARAM_H

class_model/include/class_model/gps_location.h

@@ -0,0 +1,8 @@
+#ifndef GOBAL_LOCATION_H
+#define GOBAL_LOCATION_H
+
+typedef struct global_location{
+	double lat,lon,alt;
+}global_location;
+
+#endif // GOBAL_LOCATION_H

class_model/include/class_model/mission.h

@@ -0,0 +1,28 @@
+/*Route Mission*/
+#ifndef MISSION_H
+#define MISSION_H
+
+#include "class_model/receiver.h"
+#include "class_model/formation.h"
+#include "class_model/getParam.h"
+#include "class_model/select.h"
+
+class MissionClass {
+public:
+    MissionClass();
+    virtual ~MissionClass();
+        //ClassObject
+    ReceiverClass receiver_object;
+    FormationClass formation_object;
+    ParamClass param_object;
+    SelectClass select_formation;
+    
+    void fly2target(float x=0 ,float y=0);
+    void cruise(float x ,float y);
+    void snake(float x ,float y);
+private:
+    // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+};
+
+#endif // MISSION_H

class_model/include/class_model/mode.h

@@ -0,0 +1,23 @@
+/*Setting Pixhawk Mode*/
+#ifndef MODE_H
+#define MODE_H
+
+#include <ros/ros.h>
+#include <mavros_msgs/SetMode.h>
+
+class ModeClass {
+public:
+  ModeClass();
+  virtual ~ModeClass();
+  int set_Mode(std::string mode);
+
+private:
+  // ROS NodeHandle
+  ros::NodeHandle node_handle_;
+
+  //SERVICE
+  ros::ServiceClient set_mode_client;
+
+};
+
+#endif // MODE_H

class_model/include/class_model/receiver.h

@@ -0,0 +1,28 @@
+/*Receive Command From MQTT*/
+#ifndef RECEIVER_H
+#define RECEIVER_H
+
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+
+class ReceiverClass {
+public:
+    ReceiverClass();
+    virtual ~ReceiverClass();
+    std::string check_command();
+
+private:
+    // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+
+    std::string mqtt_command;
+
+    //SERVICE
+    //SUBSCRIBE
+    ros::Subscriber mqtt_sub;
+
+    void cmd_receiver(const std_msgs::String::ConstPtr &msg);
+
+};
+
+#endif // RECEIVER_H

class_model/include/class_model/requestData.h

@@ -0,0 +1,31 @@
+/*Subscribe Data Which MQTT Pubilsh */
+#ifndef REQUESTDATA_H
+#define REQUESTDATA_H
+
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <string>
+#include <class_model/gps_location.h>
+
+class RequestClass {
+public:
+    RequestClass();
+    virtual ~RequestClass();
+    global_location get_leader_GPS();
+    float get_leader_heading();
+    
+private:
+  // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+
+    void Data_callback(const std_msgs::String::ConstPtr &gps);
+    void jsonToInt(std::string data);
+    float heading;
+  // SUBSCRIBE
+    ros::Subscriber mqtt_data; 
+
+
+};
+
+#endif // REQUESTDATA_H
+

class_model/include/class_model/select.h

@@ -0,0 +1,33 @@
+/*Select Formation*/
+#ifndef SELECT_H
+#define SELECT_H
+
+#include "class_model/receiver.h"
+#include "class_model/formation.h"
+#include "class_model/getParam.h"
+
+class SelectClass {
+public:
+    SelectClass();
+    virtual ~SelectClass();
+        //ClassObject
+    ReceiverClass receiver_object;
+    FormationClass formation_object;
+    ParamClass param_object;
+
+    void init_formation(float x=0,float y=0);
+    void line_formation(float x=0,float y=0);
+    void row_formation();
+    void circle_formation();
+    void goose_formation(float x=0,float y=0);
+    void protect_formation(float x=0,float y=0);
+    void Hex_formation(float x=0,float y=0);
+    void stop();
+    
+private:
+    // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+    int counter;
+};
+
+#endif // SELECT_H

class_model/include/class_model/sensor.h

@@ -0,0 +1,30 @@
+/*Subscribe Pixhawk Sensor Data*/
+#ifndef SENSOR_H
+#define SENSOR_H
+
+#include <ros/ros.h>
+#include <std_msgs/Float64.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <class_model/gps_location.h>
+
+class ThreadGPSClass {
+public:
+  ThreadGPSClass();
+  virtual ~ThreadGPSClass();
+  global_location gps_position();
+  float get_heading();
+
+private:
+  // ROS NodeHandle
+  ros::NodeHandle node_handle_;
+
+  // SUBSCRIBE
+  ros::Subscriber gps_sub;
+  ros::Subscriber compass_sub;
+
+  void GPS_callback(const sensor_msgs::NavSatFix::ConstPtr &gps);
+  void Compass_callback(const std_msgs::Float64::ConstPtr &degree);
+
+};
+
+#endif // SENSOR_H