ver4

class_model/CMakeLists.txt

@@ -245,7 +245,7 @@ target_include_directories(requestData_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/in
 target_link_libraries(requestData_lib ${catkin_LIBRARIES})
 add_dependencies(requestData_lib main_generate_messages_cpp)
 
-add_library(param_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/getParam.cpp)
+add_library(param_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/Param.cpp)
 target_include_directories(param_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
 target_link_libraries(param_lib ${catkin_LIBRARIES})
 add_dependencies(param_lib main_generate_messages_cpp)

class_model/include/class_model/Param.h

@@ -1,6 +1,6 @@
-/*Get Drone's Parameter*/
-#ifndef GETPARAM_H
-#define GETPARAM_H
+/*Get/SET Drone's Parameter*/
+#ifndef PARAM_H
+#define PARAM_H
 
 #include <ros/ros.h>
 
@@ -9,6 +9,8 @@ public:
     ParamClass();
     virtual ~ParamClass();
     int getID();
+    int setParam(std::string ParamName , int value);
+    int getParam(std::string ParamName);
 
 private:
     // ROS NodeHandle
@@ -25,4 +27,4 @@ private:
 
 };
 
-#endif // GETPARAM_H
+#endif // PARAM_H

class_model/include/class_model/choose_leader.h

@@ -0,0 +1,22 @@
+// choose a leader , other drones set as follower
+#ifndef CHOOSE_LEADER_H
+#define CHOOSE_LEADER_H
+
+#include <ros/ros.h>
+#include "class_model/Param.h"
+
+class SelectionClass{
+public:
+    SelectionClass();
+    virtual ~SelectionClass();
+
+    ParamClass param_object;
+    void choose_leader();
+
+private:
+    // ROS NodeHandle
+    ros::NodeHandle node_handle_;
+    int ID;
+
+};
+#endif //CHOOSE_LEADER_H

class_model/include/class_model/mission.h

@@ -4,7 +4,7 @@
 
 #include "class_model/receiver.h"
 #include "class_model/formation.h"
-#include "class_model/getParam.h"
+#include "class_model/Param.h"
 #include "class_model/select.h"
 
 class MissionClass {
@@ -20,6 +20,9 @@ public:
     void fly2target(float x=0 ,float y=0);
     void cruise(float x ,float y);
     void snake(float x ,float y);
+    void start();
+    void set_mission();
+
 private:
     // ROS NodeHandle
     ros::NodeHandle node_handle_;

class_model/include/class_model/requestData.h

@@ -7,7 +7,7 @@
 #include <string>
 #include <class_model/gps_location.h>
 #include <nlohmann/json.hpp>
-#include "rapidjson/document.h"
+// #include "rapidjson/document.h"
 
 using json = nlohmann::json;
 
@@ -32,7 +32,7 @@ private:
     void StringToJson(std::string data);
     float heading;
     global_location leader_position;
-    rapidjson::Document document;
+    // rapidjson::Document document;
     json j_data;
 
   // SUBSCRIBE

class_model/include/class_model/select.h

@@ -4,7 +4,7 @@
 
 #include "class_model/receiver.h"
 #include "class_model/formation.h"
-#include "class_model/getParam.h"
+#include "class_model/Param.h"
 
 class SelectClass {
 public:
@@ -22,12 +22,14 @@ public:
     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 start_formation();
     void stop();
     
 private:
     // ROS NodeHandle
     ros::NodeHandle node_handle_;
     int counter;
+    int JobNumber;
 };
 
 #endif // SELECT_H

class_model/src/Param.cpp

@@ -0,0 +1,51 @@
+#include"class_model/Param.h"
+
+ParamClass::ParamClass() : node_handle_("~"){
+
+
+}
+
+ParamClass::~ParamClass() { ros::shutdown(); }
+
+int ParamClass::getID() {
+	
+    int ParamData;
+	if (!node_handle_.hasParam("droneID"))
+	{
+		ROS_INFO("No Param Named droneID");
+        return 0;
+	}else{
+		node_handle_.getParam("droneID", ParamData);
+		// ROS_INFO("Drone ID %d", ParamData);
+        return ParamData;
+	}
+
+}
+
+int ParamClass::getParam(std::string ParamName) {
+	
+    int ParamData;
+	if (!node_handle_.hasParam(ParamName))
+	{
+		ROS_INFO("No Param Named %s" , ParamName);
+        return 0;
+	}else{
+		node_handle_.getParam(ParamName, ParamData);
+		// ROS_INFO("Drone ID %d", ParamData);
+        return ParamData;
+	}
+
+}
+
+int ParamClass::setParam(std::string ParamName , int value){
+		
+	if (!node_handle_.hasParam(ParamName))
+	{
+		ROS_INFO("No Param Named %s" , ParamName);
+        
+	}else{
+		node_handle_.setParam(ParamName,value);
+		
+        return 0;
+	}
+}

class_model/src/choose_leader.cpp

@@ -0,0 +1,22 @@
+#include "class_model/choose_leader.h"
+
+
+SelectionClass::SelectionClass() : node_handle_(""){
+    
+
+}
+
+SelectionClass::~SelectionClass() { ros::shutdown(); }
+
+void SelectionClass::choose_leader(){
+
+    ID = param_object.getID();
+
+    if(ID == 0){
+        param_object.setParam("JobNumber",0);
+    }else {
+        param_object.setParam("JobNumber", ID);       
+    }
+
+
+}

class_model/src/getParam.cpp

@@ -1,24 +0,0 @@
-#include"class_model/getParam.h"
-
-ParamClass::ParamClass() : node_handle_("~"){
-
-
-}
-
-ParamClass::~ParamClass() { ros::shutdown(); }
-
-int ParamClass::getID() {
-	
-    int ParamData;
-	if (!node_handle_.hasParam("droneID"))
-	{
-		ROS_INFO("No Param Named droneID");
-        return 0;
-	}else{
-		node_handle_.getParam("droneID", ParamData);
-		// ROS_INFO("Drone ID %d", ParamData);
-        return ParamData;
-	}
-
-
-}

class_model/src/local_mqtt_pub_data_to_ros.py

@@ -11,7 +11,7 @@ import json
 
 # Ros
 def ros_pub(dataJson):
-    global publisher, rate
+    # global publisher, rate
     # data = json.loads(dataJson)
     publisher.publish(dataJson)     #將date字串發布到topic
     rate.sleep()

class_model/src/main.cpp

@@ -1,4 +1,5 @@
 #include "class_model/mission.h"
+#include "class_model/choose_leader.h"
 
 int main(int argc, char **argv) {
 
@@ -9,13 +10,43 @@ int main(int argc, char **argv) {
     ros::AsyncSpinner spinner(0);
 	spinner.start();   
 
-    ThreadGPSClass gps_object;
+    ModeClass mode_object;
+    ControlClass control_object;
+    ReceiverClass receiver_object;
+    MissionClass mission_object;
+    CommandClass command_object;
+    SelectionClass selection_object;
+    
+    
+    mode_object.set_Mode("GUIDED");
+    control_object.arm();
+    control_object.takeoff(4.5);
+    sleep(5);
+
+    selection_object.choose_leader();
+    // mission_object.start();   //default hover in goose formation
+
+    while(ros::ok()){
+
+    //json_object.get_command();       //for follower
+
+
+
+    ros::waitForShutdown();
+
+    return 0;
+}
+
+
+/*
+ThreadGPSClass gps_object;
     ModeClass mode_object;
     ControlClass control_object;
     SelectClass select_formation;
     ReceiverClass receiver_object;
     MissionClass mission_object;
     CommandClass command_object;
+    
     std::string type = "";
 
     mode_object.set_Mode("GUIDED");
@@ -60,13 +91,4 @@ int main(int argc, char **argv) {
         mode_object.set_Mode("LAND");
     }
      }
-
-    
-    // select_formation.square();
-
-    // RequestClass test_object;
-
-    ros::waitForShutdown();
-
-    return 0;
-}
+*/

class_model/src/mission.cpp

@@ -47,4 +47,10 @@ void MissionClass::snake(float x, float y){
     sleep(1);
     select_formation.line_formation(x,0);
     sleep(1);
+}
+
+void MissionClass::start(){
+       
+    select_formation.start_formation();
+
 }

class_model/src/requestData.cpp

@@ -2,7 +2,8 @@
 #include <cstdlib>
 #include <iostream>
 
-int command; 
+int command;
+// json j_data;   //Don't set json as global variable
 
 RequestClass::RequestClass() : node_handle_(""){
 
@@ -16,14 +17,7 @@ RequestClass::RequestClass() : node_handle_(""){
 RequestClass::~RequestClass() { ros::shutdown(); }
 
 void RequestClass::Data_callback(const std_msgs::String::ConstPtr &sensor) {
-
-
   StringToJson(sensor->data);
-
-  // std::string data = sensor->data;
-  // jsonToInt(data);
-
-
 }
 
 void RequestClass::Message_callback(const std_msgs::String::ConstPtr &message) {
@@ -49,55 +43,6 @@ int RequestClass::get_formation_message(){
   return command;
 }
 
-// json RequestClass::get_data(){
-
-//   return data;
-// }
-
-void RequestClass::jsonToInt(std::string data){
-
-  std::string lat,lon,alt,degree;
-  std::string list[5]={"","","","",""};
-  int j = 0;
-
-  // lat.append(data,9,10);
-  // lon.append(data,29,10);
-  // degree.append(data,53,6);
-
-  // heading = std::stoi(degree);
-  // leader_position.lat=std::stoi(lat);   
-	// leader_position.lon=std::stoi(lon);
-
-  for(int i=0;i<data.length();i++){
-    if(data[i] == ':'){
-      i+=3;
-      while(data[i] != ','){
-        list[j]=list[j]+data[i];
-        i++; 
-        if(data[i] == ',' || data[i] == '}'){
-            j++;
-            break;
-        }
-        if(j>3){
-            break;
-        }     
-      }
-    }
-  }
-
-  leader_position.lat=std::stoi(list[0]);   
-	leader_position.lon=std::stoi(list[1]);
-  leader_position.alt=std::stoi(list[2]);
-  heading = std::stoi(list[3]);
-
-
-
-  // std::cout<<sizeof(data)<<std::endl;
-  //std::cout<<data<<std::endl;
-  // ROS_INFO("leader_GPS [%f,%f]", leader_position.lat, leader_position.lon);
-  //ROS_INFO("leader_heading: %f",heading);
-}
-
 void RequestClass::jsonToString(std::string data){
 
   std::string list[1]={""};
@@ -130,7 +75,7 @@ void RequestClass::jsonToString(std::string data){
 
 void RequestClass::StringToJson(std::string data){
 	
-  std::cout << data << std::endl;
+  std::cout <<"string: "<< data << std::endl;
 
   // document.Parse(data.c_str());                  //china's library
   // leader_position.lat=document["lat"].GetInt();   
@@ -157,9 +102,9 @@ void RequestClass::StringToJson(std::string data){
   leader_position.alt=j_data["alt"];
   heading = j_data["heading"];
   
-  //std::cout << "sensor" << sensor->data << std::endl;
-  std::cout << leader_position.lat << std::endl;
-  std::cout << leader_position.lon << std::endl;
-  std::cout << leader_position.alt << std::endl;
-  std::cout << heading << std::endl;
+  std::cout <<"Json: " <<j_data << std::endl;
+  // std::cout << leader_position.lat << std::endl;
+  // std::cout << leader_position.lon << std::endl;
+  // std::cout << leader_position.alt << std::endl;
+  // std::cout << heading << std::endl;
 }

class_model/src/select.cpp

@@ -173,6 +173,24 @@ void SelectClass::Hex_formation(float x ,float y){
     // mode_object.set_Mode("LAND");
 }
 
+void SelectClass::start_formation(){
+
+    JobNumber = param_object.getParam("JobNumber");
+    if(JobNumber == 0){
+        formation_object.leader1();
+    }else if(JobNumber == 1){
+        formation_object.follower1(counter);
+    }else if(JobNumber == 2){
+        formation_object.follower2(counter);
+    }else if(JobNumber == 3){
+        formation_object.follower3(counter);
+    }else if(JobNumber == 4){
+        formation_object.follower4(counter);
+    }else if(JobNumber == 5){
+        formation_object.follower5(counter);
+    }
+}
+
 void SelectClass::stop(){
 
 }