diff --git a/class_model/.vscode/settings.json b/class_model/.vscode/settings.json index d1fc194..4fe0732 100644 --- a/class_model/.vscode/settings.json +++ b/class_model/.vscode/settings.json @@ -4,5 +4,73 @@ ], "python.analysis.extraPaths": [ "/opt/ros/noetic/lib/python3/dist-packages" - ] + ], + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "hash_map": "cpp", + "strstream": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "hash_set": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "variant": "cpp", + "*.ipp": "cpp" + } } \ No newline at end of file diff --git a/class_model/include/class_model/requestData.h b/class_model/include/class_model/requestData.h index 79be86f..d1d03f0 100755 --- a/class_model/include/class_model/requestData.h +++ b/class_model/include/class_model/requestData.h @@ -13,17 +13,20 @@ public: virtual ~RequestClass(); global_location get_leader_GPS(); float get_leader_heading(); + std::string get_formation_message(); private: // ROS NodeHandle ros::NodeHandle node_handle_; void Data_callback(const std_msgs::String::ConstPtr &gps); + void Message_callback(const std_msgs::String::ConstPtr &message); void jsonToInt(std::string data); + void jsonToString(std::string data); float heading; // SUBSCRIBE ros::Subscriber mqtt_data; - + ros::Subscriber formation_data; }; diff --git a/class_model/include/class_model/select.h b/class_model/include/class_model/select.h index 25d310a..5e635e0 100755 --- a/class_model/include/class_model/select.h +++ b/class_model/include/class_model/select.h @@ -17,7 +17,7 @@ public: void init_formation(float x=0,float y=0); void line_formation(float x=0,float y=0); - void row_formation(); + void row_formation(float x=0,float y=0); void circle_formation(); void goose_formation(float x=0,float y=0); void protect_formation(float x=0,float y=0); diff --git a/class_model/src/formation.cpp b/class_model/src/formation.cpp index e2cc4cf..6e2cd35 100755 --- a/class_model/src/formation.cpp +++ b/class_model/src/formation.cpp @@ -65,10 +65,50 @@ void FormationClass::leader(){ } } +void FormationClass::leader1(float x,float y){ + + std::string command = "",pre_command = ""; + std_msgs::String message; + + leader_location=request_object.get_leader_GPS(); + target_location.lon = leader_location.lon/100 + x; + target_location.lat = leader_location.lat/100 + y/1.1; + sleep(3); + flag = 0; + heading_status = 0; + message.data = "1"; + while(true){ + if(flag==0){ + go2target(x,y); + }else{ + next_command.publish(message); + break; + } + + command = receiver_object.check_command(); + while(command =="stop" || command == "land"){ + command_object.set_velocity(0,0,0,0,1); + command = receiver_object.check_command(); + + if(command == "land"){ + mode_object.set_Mode("LAND"); + }else if(command != "stop"){ + break; + } + + } + + if(command != pre_command){ + // ROS_INFO("change formation"); + break; + } + pre_command = command; + } +} void FormationClass::follower1(int type){ - std::string command,pre_command = ""; + std::string message,command,pre_command = ""; while(true){ if(type == 0){ @@ -86,7 +126,9 @@ void FormationClass::follower1(int type){ } command = receiver_object.check_command(); - if(command != pre_command){ + message = request_object.get_formation_message(); + + if(command != pre_command || message == "1"){ // ROS_INFO("change formation"); break; } @@ -103,7 +145,7 @@ void FormationClass::follower1(int type){ void FormationClass::follower2(int type){ - std::string command,pre_command = ""; + std::string message,command,pre_command = ""; while(true){ if(type == 0){ @@ -121,7 +163,9 @@ void FormationClass::follower2(int type){ } command = receiver_object.check_command(); - if(command != pre_command){ + message = request_object.get_formation_message(); + + if(command != pre_command || message == "1"){ // ROS_INFO("change formation"); break; } @@ -134,7 +178,7 @@ void FormationClass::follower2(int type){ void FormationClass::follower3(int type){ - std::string command,pre_command = ""; + std::string message,command,pre_command = ""; while(true){ if(type == 0){ @@ -153,7 +197,9 @@ void FormationClass::follower3(int type){ command = receiver_object.check_command(); - if(command != pre_command){ + message = request_object.get_formation_message(); + + if(command != pre_command || message == "1"){ // ROS_INFO("change formation"); break; } @@ -166,7 +212,7 @@ void FormationClass::follower3(int type){ void FormationClass::follower4(int type){ - std::string command,pre_command = ""; + std::string message,command,pre_command = ""; while(true){ if(type == 0){ @@ -184,12 +230,13 @@ void FormationClass::follower4(int type){ } command = receiver_object.check_command(); - if(command != pre_command){ + message = request_object.get_formation_message(); + + if(command != pre_command || message == "1"){ // ROS_INFO("change formation"); break; } pre_command = command; - } @@ -197,7 +244,7 @@ void FormationClass::follower4(int type){ void FormationClass::follower5(int type){ - std::string command,pre_command = ""; + std::string message,command,pre_command = ""; while(true){ if(type == 0){ @@ -215,7 +262,9 @@ void FormationClass::follower5(int type){ } command = receiver_object.check_command(); - if(command != pre_command){ + message = request_object.get_formation_message(); + + if(command != pre_command || message == "1"){ // ROS_INFO("change formation"); break; } @@ -228,7 +277,7 @@ void FormationClass::follower5(int type){ void FormationClass::sph_follower1(int type){ - std::string command,pre_command = ""; + std::string message,command,pre_command = ""; while(true){ if(type == 0){ @@ -245,7 +294,9 @@ void FormationClass::sph_follower1(int type){ command = receiver_object.check_command(); - if(command != pre_command){ + message = request_object.get_formation_message(); + + if(command != pre_command || message == "1"){ // ROS_INFO("change formation"); break; } @@ -258,7 +309,7 @@ void FormationClass::sph_follower1(int type){ void FormationClass::sph_follower2(int type){ - std::string command,pre_command = ""; + std::string message,command,pre_command = ""; while(true){ if(type == 0){ @@ -275,12 +326,13 @@ void FormationClass::sph_follower2(int type){ command = receiver_object.check_command(); - if(command != pre_command){ + message = request_object.get_formation_message(); + + if(command != pre_command || message == "1"){ // ROS_INFO("change formation"); break; } pre_command = command; - } @@ -288,7 +340,7 @@ void FormationClass::sph_follower2(int type){ void FormationClass::sph_follower3(int type){ - std::string command,pre_command = ""; + std::string message,command,pre_command = ""; while(true){ if(type == 0){ @@ -305,7 +357,9 @@ void FormationClass::sph_follower3(int type){ command = receiver_object.check_command(); - if(command != pre_command){ + message = request_object.get_formation_message(); + + if(command != pre_command || message == "1"){ // ROS_INFO("change formation"); break; } @@ -317,7 +371,7 @@ void FormationClass::sph_follower3(int type){ void FormationClass::sph_follower4(int type){ - std::string command,pre_command = ""; + std::string message,command,pre_command = ""; while(true){ if(type == 0){ @@ -334,7 +388,9 @@ void FormationClass::sph_follower4(int type){ command = receiver_object.check_command(); - if(command != pre_command){ + message = request_object.get_formation_message(); + + if(command != pre_command || message == "1"){ // ROS_INFO("change formation"); break; } @@ -345,43 +401,7 @@ void FormationClass::sph_follower4(int type){ } -void FormationClass::leader1(float x,float y){ - - std::string command = "",pre_command = ""; - leader_location=request_object.get_leader_GPS(); - target_location.lon = leader_location.lon/100 + x; - target_location.lat = leader_location.lat/100 + y/1.1; - sleep(3); - flag = 0; - heading_status = 0; - while(true){ - if(flag==0){ - go2target(x,y); - }else{ - next_command.publish("1"); - break; - } - - command = receiver_object.check_command(); - while(command =="stop" || command == "land"){ - command_object.set_velocity(0,0,0,0,1); - command = receiver_object.check_command(); - - if(command == "land"){ - mode_object.set_Mode("LAND"); - }else if(command != "stop"){ - break; - } - - } - if(command != pre_command){ - // ROS_INFO("change formation"); - break; - } - pre_command = command; - } -} void FormationClass::calculate_position(float k,float theta,int direction){ diff --git a/class_model/src/local_mqtt_pub_data_to_ros.py b/class_model/src/local_mqtt_pub_data_to_ros.py index d6f8798..9ebf8ba 100755 --- a/class_model/src/local_mqtt_pub_data_to_ros.py +++ b/class_model/src/local_mqtt_pub_data_to_ros.py @@ -16,6 +16,12 @@ def ros_pub(dataJson): rate.sleep() # print(f"publish data {data}") +def PubMessage(dataJson): + global publisher1 + # data = json.loads(dataJson) + publisher1.publish(dataJson) #將date字串發布到topic + rate.sleep() + # MQTT def initialise_clients(clientname): # callback assignment @@ -26,18 +32,21 @@ def initialise_clients(clientname): def on_connect(client, userdata, flags, rc): print("Connected with result code " + str(rc)) # client.subscribe(mqtt_config["topic"]) - client.subscribe("data/imu") + client.subscribe("data/sensor") + client.subscribe("data/message") def on_message(client, userdata, msg): # print(f"got {msg.payload.decode('utf-8')}") # print(f"msg.topic {msg.payload.decode('utf-8')}") ros_pub(msg.payload.decode('utf-8')) + PubMessage(msg.payload.decode('utf-8')) if __name__ == '__main__': # Mqtt - mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/imu"} + mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor", "topic1":"data/message"} + client = initialise_clients("123456") client.on_connect = on_connect client.on_message = on_message @@ -50,7 +59,10 @@ if __name__ == '__main__': rospy.init_node(Mqtt_Node) # initialize Ros node topicName = 'uav_message' + topicName1 = 'formation_message' publisher = rospy.Publisher(topicName,String,queue_size=10) + publisher1 = rospy.Publisher(topicName1,String,queue_size=10) + rate = rospy.Rate(10) diff --git a/class_model/src/local_mqtt_sub_data_from_ros.py b/class_model/src/local_mqtt_sub_data_from_ros.py index b35f8f5..f1422f1 100755 --- a/class_model/src/local_mqtt_sub_data_from_ros.py +++ b/class_model/src/local_mqtt_sub_data_from_ros.py @@ -16,7 +16,7 @@ from sensor_msgs.msg import Imu from sensor_msgs.msg import Range from geometry_msgs.msg import Vector3 -mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/imu"} +mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor", "topic1": "data/message"} data ={} # Ros def callBack_imu(IMU): @@ -57,6 +57,15 @@ def callBack_state(state): dataGpsUpdate = {"mode": mode} data.update(dataGpsUpdate) + +def callBack_message(data): + MessageUpdate = {"message": data} + data.update(MessageUpdate) + dataMessageFormate = json.dumps(data) + mqtt_Pub_message(message=dataMessageFormate) + + +# get parameter def GetParam(self,param_name): param = self.get_param_srv(param_name) if param.success: @@ -90,6 +99,14 @@ def mqtt_Pub(message, topics = mqtt_config["topic"], waitForAck=False): time.sleep(0.25) client.topic_ack.remove(mid) +def mqtt_Pub_message(message, topics = mqtt_config["topic1"], waitForAck=False): + mid = client.publish(topics, message, 0)[1] + # print(f"just published {message} to topic") + if waitForAck: + while mid not in client.topic_ack: + print("wait for ack") + time.sleep(0.25) + client.topic_ack.remove(mid) def on_publish(self, userdata, mid): client.topic_ack.append(mid) @@ -122,8 +139,9 @@ if __name__ == '__main__': # subscriber = rospy.Subscriber('/mavros/imu/data_raw',Imu,callBack_imu) #從topic獲取string再呼叫callback subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/global',NavSatFix,callBack_gps) #從topic獲取string再呼叫callback subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg',Float64,callBack_compass_hdg) #從topic獲取string再呼叫callback + subscriber = rospy.Subscriber(ros_namespace+"/mavros/next_command",String,callBack_message) #subscriber = rospy.Subscriber(ros_namespace+'/mavros/mavros_msgs/State',Header,callBack_state) - ID = rospy.get_param(ros_namespace+'/leader/droneID') + # ID = rospy.get_param(ros_namespace+'/leader/droneID') # subscriber = rospy.Subscriber('/mavros/rangefinder/rangefinder',Range,callBack_rng) #從topic獲取string再呼叫callback # ID = rospy.GetParam("droneID") diff --git a/class_model/src/main.cpp b/class_model/src/main.cpp index f53e4d4..e53b8bf 100755 --- a/class_model/src/main.cpp +++ b/class_model/src/main.cpp @@ -28,29 +28,29 @@ int main(int argc, char **argv) { type = receiver_object.check_command(); // // ROS_INFO("%s",type.c_str()); - if(type == "" || type == "init"){ - select_formation.goose_formation(); - ROS_INFO("init formation"); - }else if(type == "line"){ - select_formation.line_formation(); - // ROS_INFO("line foemation"); - }else if(type == "row"){ - select_formation.row_formation(); - }else if(type == "circle"){ - select_formation.circle_formation(); - }else if(type == "stop"){ - select_formation.stop(); - }else if(type == "land"){ - mode_object.set_Mode("LAND"); - } + // if(type == "" || type == "init"){ + // select_formation.goose_formation(); + // ROS_INFO("init formation"); + // }else if(type == "line"){ + // select_formation.line_formation(); + // // ROS_INFO("line foemation"); + // }else if(type == "row"){ + // select_formation.row_formation(); + // }else if(type == "circle"){ + // select_formation.circle_formation(); + // }else if(type == "stop"){ + // select_formation.stop(); + // }else if(type == "land"){ + // mode_object.set_Mode("LAND"); + // } //mission_object.cruise(20,20); - // select_formation.goose_formation(); + select_formation.goose_formation(5,0); // }else if (type == "hex"){ // select_formation.Hex_formation(); // } - // select_formation.Hex_formation(); + select_formation.row_formation(-5,0); sleep(0.5); // command_object.set_velocity(0 ,0 ,0,0,1); diff --git a/class_model/src/requestData.cpp b/class_model/src/requestData.cpp index 43370f1..93720d5 100755 --- a/class_model/src/requestData.cpp +++ b/class_model/src/requestData.cpp @@ -2,12 +2,15 @@ #include #include -global_location leader_position; +global_location leader_position; +std::string command; RequestClass::RequestClass() : node_handle_(""){ mqtt_data = node_handle_.subscribe("/uav_message", 10, &RequestClass::Data_callback, this); + formation_data = node_handle_.subscribe("/formation_message", 10, + &RequestClass::Message_callback, this); } @@ -20,6 +23,13 @@ void RequestClass::Data_callback(const std_msgs::String::ConstPtr &sensor) { } +void RequestClass::Message_callback(const std_msgs::String::ConstPtr &message) { + + std::string data = message->data; + + +} + global_location RequestClass::get_leader_GPS(){ return leader_position; @@ -30,6 +40,11 @@ float RequestClass::get_leader_heading(){ return heading; } +std::string RequestClass::get_formation_message(){ + + return command; +} + void RequestClass::jsonToInt(std::string data){ std::string lat,lon,degree; @@ -68,6 +83,40 @@ void RequestClass::jsonToInt(std::string data){ + // std::cout<sizeof(list)){ + break; + } + } + } + } + + command = list[1]; + + // std::cout<