From 62bea117ee88c016b4e361b2147e5ec5feae6358 Mon Sep 17 00:00:00 2001 From: dodo Date: Mon, 14 Nov 2022 20:31:08 +0800 Subject: [PATCH] first commit --- class_model/.vscode/c_cpp_properties.json | 23 + class_model/.vscode/settings.json | 8 + class_model/CMakeLists.txt | 279 ++++++++ class_model/include/class_model/PID.h | 27 + class_model/include/class_model/command.h | 45 ++ class_model/include/class_model/control.h | 34 + .../include/class_model/convert_degree.h | 41 ++ class_model/include/class_model/follower.h | 36 ++ class_model/include/class_model/formation.h | 68 ++ class_model/include/class_model/getParam.h | 28 + .../include/class_model/gps_location.h | 8 + class_model/include/class_model/mission.h | 28 + class_model/include/class_model/mode.h | 23 + class_model/include/class_model/receiver.h | 28 + class_model/include/class_model/requestData.h | 31 + class_model/include/class_model/select.h | 33 + class_model/include/class_model/sensor.h | 30 + class_model/launch/apm.launch | 27 + class_model/launch/apm_sitl.launch | 26 + class_model/launch/five.launch | 45 ++ class_model/launch/six.launch | 54 ++ class_model/launch/test.launch | 19 + class_model/package.xml | 68 ++ class_model/src/PID.cpp | 70 ++ class_model/src/PID.py | 49 ++ class_model/src/command.cpp | 184 ++++++ class_model/src/command.py | 77 +++ class_model/src/control.cpp | 60 ++ class_model/src/drone2.cpp | 19 + class_model/src/follower.cpp | 64 ++ class_model/src/formation.cpp | 604 ++++++++++++++++++ class_model/src/getParam.cpp | 24 + class_model/src/local_mqtt_pub_data_to_ros.py | 67 ++ .../src/local_mqtt_sub_data_from_ros.py | 132 ++++ class_model/src/main.cpp | 72 +++ class_model/src/mission.cpp | 50 ++ class_model/src/mode.cpp | 33 + class_model/src/receiver.cpp | 26 + class_model/src/requestData.cpp | 75 +++ class_model/src/select.cpp | 178 ++++++ class_model/src/sensor.cpp | 45 ++ class_model/src/tower.py | 59 ++ 42 files changed, 2897 insertions(+) create mode 100644 class_model/.vscode/c_cpp_properties.json create mode 100644 class_model/.vscode/settings.json create mode 100755 class_model/CMakeLists.txt create mode 100755 class_model/include/class_model/PID.h create mode 100755 class_model/include/class_model/command.h create mode 100755 class_model/include/class_model/control.h create mode 100755 class_model/include/class_model/convert_degree.h create mode 100755 class_model/include/class_model/follower.h create mode 100755 class_model/include/class_model/formation.h create mode 100755 class_model/include/class_model/getParam.h create mode 100755 class_model/include/class_model/gps_location.h create mode 100755 class_model/include/class_model/mission.h create mode 100755 class_model/include/class_model/mode.h create mode 100755 class_model/include/class_model/receiver.h create mode 100755 class_model/include/class_model/requestData.h create mode 100755 class_model/include/class_model/select.h create mode 100755 class_model/include/class_model/sensor.h create mode 100755 class_model/launch/apm.launch create mode 100755 class_model/launch/apm_sitl.launch create mode 100755 class_model/launch/five.launch create mode 100755 class_model/launch/six.launch create mode 100755 class_model/launch/test.launch create mode 100755 class_model/package.xml create mode 100755 class_model/src/PID.cpp create mode 100755 class_model/src/PID.py create mode 100755 class_model/src/command.cpp create mode 100755 class_model/src/command.py create mode 100755 class_model/src/control.cpp create mode 100755 class_model/src/drone2.cpp create mode 100755 class_model/src/follower.cpp create mode 100755 class_model/src/formation.cpp create mode 100755 class_model/src/getParam.cpp create mode 100755 class_model/src/local_mqtt_pub_data_to_ros.py create mode 100755 class_model/src/local_mqtt_sub_data_from_ros.py create mode 100755 class_model/src/main.cpp create mode 100755 class_model/src/mission.cpp create mode 100755 class_model/src/mode.cpp create mode 100755 class_model/src/receiver.cpp create mode 100755 class_model/src/requestData.cpp create mode 100755 class_model/src/select.cpp create mode 100755 class_model/src/sensor.cpp create mode 100755 class_model/src/tower.py diff --git a/class_model/.vscode/c_cpp_properties.json b/class_model/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..565353c --- /dev/null +++ b/class_model/.vscode/c_cpp_properties.json @@ -0,0 +1,23 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/opt/ros/noetic/include/**", + "/home/dodo/ardupilot_ws/src/ROS_controll/include/**", + "/home/dodo/ardupilot_ws/src/class_model/include/**", + "/home/dodo/ardupilot_ws/src/iq_gnc/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/class_model/.vscode/settings.json b/class_model/.vscode/settings.json new file mode 100644 index 0000000..d1fc194 --- /dev/null +++ b/class_model/.vscode/settings.json @@ -0,0 +1,8 @@ +{ + "python.autoComplete.extraPaths": [ + "/opt/ros/noetic/lib/python3/dist-packages" + ], + "python.analysis.extraPaths": [ + "/opt/ros/noetic/lib/python3/dist-packages" + ] +} \ No newline at end of file diff --git a/class_model/CMakeLists.txt b/class_model/CMakeLists.txt new file mode 100755 index 0000000..2606293 --- /dev/null +++ b/class_model/CMakeLists.txt @@ -0,0 +1,279 @@ +cmake_minimum_required(VERSION 3.0.2) +project(class_model) + +SET(CMAKE_BUILD_TYPE Debug) +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES class_model + CATKIN_DEPENDS roscpp rospy std_msgs + DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/class_model.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/class_model_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_class_model.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) + +#add_library(sensor_lib src/sensor.cpp) + +#add_executable(main src/main.cpp) +#target_link_libraries(main ${catkin_LIBRARIES} sensor_lib) + +#add_executable(sensor src/sensor.cpp) +#target_link_libraries(sensor ${catkin_LIBRARIES}) + + +add_library(sensor_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/sensor.cpp) +target_include_directories(sensor_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) +target_link_libraries(sensor_lib ${catkin_LIBRARIES}) +add_dependencies(sensor_lib main_generate_messages_cpp) + +add_library(mode_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/mode.cpp) +target_include_directories(mode_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) +target_link_libraries(mode_lib ${catkin_LIBRARIES}) +add_dependencies(mode_lib main_generate_messages_cpp) + +add_library(control_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/control.cpp) +target_include_directories(control_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) +target_link_libraries(control_lib ${catkin_LIBRARIES} sensor_lib) +add_dependencies(control_lib main_generate_messages_cpp) + +add_library(command_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/command.cpp) +target_include_directories(command_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) +target_link_libraries(command_lib ${catkin_LIBRARIES}) +add_dependencies(command_lib main_generate_messages_cpp) + +add_library(receiver_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/receiver.cpp) +target_include_directories(receiver_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) +target_link_libraries(receiver_lib ${catkin_LIBRARIES}) +add_dependencies(receiver_lib main_generate_messages_cpp) + +add_library(requestData_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/requestData.cpp) +target_include_directories(requestData_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) +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) +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) + +add_library(PID_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/PID.cpp) +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(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) +add_dependencies(formation_lib main_generate_messages_cpp) + +add_library(select_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/select.cpp) +target_include_directories(select_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) +target_link_libraries(select_lib ${catkin_LIBRARIES} receiver_lib formation_lib param_lib) +add_dependencies(select_lib main_generate_messages_cpp) + +add_library(mission_lib ${CMAKE_CURRENT_SOURCE_DIR}/src/mission.cpp) +target_include_directories(mission_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) +target_link_libraries(mission_lib ${catkin_LIBRARIES} select_lib) +add_dependencies(mission_lib main_generate_messages_cpp) + +################## +add_executable(main src/main.cpp) +target_link_libraries(main ${catkin_LIBRARIES} mission_lib) +add_dependencies(main class_model_generate_messages_cpp) + diff --git a/class_model/include/class_model/PID.h b/class_model/include/class_model/PID.h new file mode 100755 index 0000000..e873161 --- /dev/null +++ b/class_model/include/class_model/PID.h @@ -0,0 +1,27 @@ +/*PID Controlier*/ +#ifndef PID_H +#define PID_H + +#include +#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 \ No newline at end of file diff --git a/class_model/include/class_model/command.h b/class_model/include/class_model/command.h new file mode 100755 index 0000000..c5a670a --- /dev/null +++ b/class_model/include/class_model/command.h @@ -0,0 +1,45 @@ +/*Drone Control Command*/ +#ifndef COMMAND_H +#define COMMAND_H + +#include +#include +#include +#include + +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 \ No newline at end of file diff --git a/class_model/include/class_model/control.h b/class_model/include/class_model/control.h new file mode 100755 index 0000000..a379c1c --- /dev/null +++ b/class_model/include/class_model/control.h @@ -0,0 +1,34 @@ +#ifndef CONTROL_H +#define CONTROL_H + +#include +#include +#include +#include + +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 \ No newline at end of file diff --git a/class_model/include/class_model/convert_degree.h b/class_model/include/class_model/convert_degree.h new file mode 100755 index 0000000..e05ec93 --- /dev/null +++ b/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; +} \ No newline at end of file diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h new file mode 100755 index 0000000..28d1042 --- /dev/null +++ b/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 +#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 \ No newline at end of file diff --git a/class_model/include/class_model/formation.h b/class_model/include/class_model/formation.h new file mode 100755 index 0000000..2d8d32e --- /dev/null +++ b/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 +#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 + + + +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 \ No newline at end of file diff --git a/class_model/include/class_model/getParam.h b/class_model/include/class_model/getParam.h new file mode 100755 index 0000000..6dcc789 --- /dev/null +++ b/class_model/include/class_model/getParam.h @@ -0,0 +1,28 @@ +/*Get Drone's Parameter*/ +#ifndef GETPARAM_H +#define GETPARAM_H + +#include + +class ParamClass { +public: + ParamClass(); + virtual ~ParamClass(); + int getID(); + +private: + // ROS NodeHandle + ros::NodeHandle node_handle_; + + //SERVICE + + + //SUBSCRIBE + + + + + +}; + +#endif // GETPARAM_H \ No newline at end of file diff --git a/class_model/include/class_model/gps_location.h b/class_model/include/class_model/gps_location.h new file mode 100755 index 0000000..a7fd9b4 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/class_model/include/class_model/mission.h b/class_model/include/class_model/mission.h new file mode 100755 index 0000000..9974af3 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/class_model/include/class_model/mode.h b/class_model/include/class_model/mode.h new file mode 100755 index 0000000..7348047 --- /dev/null +++ b/class_model/include/class_model/mode.h @@ -0,0 +1,23 @@ +/*Setting Pixhawk Mode*/ +#ifndef MODE_H +#define MODE_H + +#include +#include + +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 \ No newline at end of file diff --git a/class_model/include/class_model/receiver.h b/class_model/include/class_model/receiver.h new file mode 100755 index 0000000..7132e8c --- /dev/null +++ b/class_model/include/class_model/receiver.h @@ -0,0 +1,28 @@ +/*Receive Command From MQTT*/ +#ifndef RECEIVER_H +#define RECEIVER_H + +#include +#include + +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 \ No newline at end of file diff --git a/class_model/include/class_model/requestData.h b/class_model/include/class_model/requestData.h new file mode 100755 index 0000000..79be86f --- /dev/null +++ b/class_model/include/class_model/requestData.h @@ -0,0 +1,31 @@ +/*Subscribe Data Which MQTT Pubilsh */ +#ifndef REQUESTDATA_H +#define REQUESTDATA_H + +#include +#include +#include +#include + +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 + diff --git a/class_model/include/class_model/select.h b/class_model/include/class_model/select.h new file mode 100755 index 0000000..25d310a --- /dev/null +++ b/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 \ No newline at end of file diff --git a/class_model/include/class_model/sensor.h b/class_model/include/class_model/sensor.h new file mode 100755 index 0000000..a81823c --- /dev/null +++ b/class_model/include/class_model/sensor.h @@ -0,0 +1,30 @@ +/*Subscribe Pixhawk Sensor Data*/ +#ifndef SENSOR_H +#define SENSOR_H + +#include +#include +#include +#include + +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 °ree); + +}; + +#endif // SENSOR_H \ No newline at end of file diff --git a/class_model/launch/apm.launch b/class_model/launch/apm.launch new file mode 100755 index 0000000..e077214 --- /dev/null +++ b/class_model/launch/apm.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/class_model/launch/apm_sitl.launch b/class_model/launch/apm_sitl.launch new file mode 100755 index 0000000..d1b5537 --- /dev/null +++ b/class_model/launch/apm_sitl.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/class_model/launch/five.launch b/class_model/launch/five.launch new file mode 100755 index 0000000..6575a05 --- /dev/null +++ b/class_model/launch/five.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/class_model/launch/six.launch b/class_model/launch/six.launch new file mode 100755 index 0000000..dc13342 --- /dev/null +++ b/class_model/launch/six.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/class_model/launch/test.launch b/class_model/launch/test.launch new file mode 100755 index 0000000..e8bd571 --- /dev/null +++ b/class_model/launch/test.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/class_model/package.xml b/class_model/package.xml new file mode 100755 index 0000000..2a7cba9 --- /dev/null +++ b/class_model/package.xml @@ -0,0 +1,68 @@ + + + class_model + 0.0.0 + The class_model package + + + + + dodo + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/class_model/src/PID.cpp b/class_model/src/PID.cpp new file mode 100755 index 0000000..881695b --- /dev/null +++ b/class_model/src/PID.cpp @@ -0,0 +1,70 @@ +#include "class_model/PID.h" + +// PIDClass::PIDClass() : node_handle_(""){ + + +// } + +// PIDClass::~PIDClass() { ros::shutdown(); } + +float PIDClass::update(float current ,float target ,float pidvals[3]){ + + current_time = std::clock(); + // pTime = current_time - + double t = (current_time - pTime); + float error = target - current; + + double P,D,result; + P = pidvals[0] * error; + I = I + (pidvals[1] * error *t); + D = (pidvals[2] * (error - pError))/t; + + result = P + I + D; + + if (limit !=NULL){ + if (result > limit[0]){ + result = limit[0]; + } + if (result < limit[1]){ + result = limit[1]; + } + } + + pError = error; + pTime = current_time; + + // ROS_INFO("%f,%f,%f",P,I,D); + + return result; +} + +float PIDClass::update1(float current ,float target){ + + current_time = std::clock(); + // pTime = current_time - + double t = (current_time - pTime); + float error = target - current; + + double P,D,result; + P = pidval1[0] * error; + I = I + (pidval1[1] * error *t); + D = (pidval1[2] * (error - pError))/t; + + result = P + I + D; + + if (limit !=NULL){ + if (result > limit[0]){ + result = limit[0]; + } + if (result < limit[1]){ + result = limit[1]; + } + } + + pError = error; + pTime = current_time; + + // ROS_INFO("%f,%f,%f",P,I,D); + + return result; +} \ No newline at end of file diff --git a/class_model/src/PID.py b/class_model/src/PID.py new file mode 100755 index 0000000..adf5f39 --- /dev/null +++ b/class_model/src/PID.py @@ -0,0 +1,49 @@ +import cvzone +import cv2 +import numpy as np +import time + + +class PID: + def __init__(self, pidVals, targetVal, axis=0, limit=None): + self.pidVals = pidVals + self.targetVal = targetVal + self.axis = axis + self.pError = 0 + self.limit = limit + self.I = 0 + self.pTime = 0 + + def update(self, cVal): + # Current Value - Target Value + t = time.time() - self.pTime + error = cVal - self.targetVal + P = self.pidVals[0] * error + self.I = self.I + (self.pidVals[1] * error * t) + D = (self.pidVals[2] * (error - self.pError)) / t + + result = P + self.I + D + + if self.limit is not None: + result = float(np.clip(result, self.limit[0], self.limit[1])) + self.pError = error + self.ptime = time.time() + + return result + + + +def main(): + # For a 640x480 image center target is 320 and 240 + xPID = PID([1, 0.000000000001, 1], 640 // 2) + yPID = PID([1, 0.000000000001, 1], 480 // 2, axis=1, limit=[-100, 100]) + + while True: + + xVal = int(xPID.update(cx)) + yVal = int(yPID.update(cy)) + + + +if __name__ == "__main__": + main() diff --git a/class_model/src/command.cpp b/class_model/src/command.cpp new file mode 100755 index 0000000..e615a0a --- /dev/null +++ b/class_model/src/command.cpp @@ -0,0 +1,184 @@ +#include "class_model/command.h" + +CommandClass::CommandClass() : node_handle_("~"){ + + std::string ros_namespace; + if (!node_handle_.hasParam("namespace")) + { + }else{ + node_handle_.getParam("namespace", ros_namespace); + } + + velocity_command=node_handle_.advertise(ros_namespace+"/mavros/setpoint_velocity/cmd_vel",100); + gps_command=node_handle_.advertise(ros_namespace+"/mavros/setpoint_raw/global",10); +} + +CommandClass::~CommandClass() { ros::shutdown(); } + +void CommandClass::velocity_init(){ + + msg.twist.linear.x = 0; + msg.twist.angular.x = 0; + msg.twist.linear.y = 0; + msg.twist.angular.y = 0; + msg.twist.linear.z = 0; + msg.twist.angular.z = 0; + + velocity_command.publish(msg); + + uint64_t last_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + while(true){ + velocity_command.publish(msg); + uint64_t now_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + if((now_ms-last_ms)>10){ + break; + } + } + +} + +void CommandClass::fix_velocity(float x,float y,float alt,float yaw,float second){ + + msg.twist.linear.x = x; + msg.twist.angular.x = x; + msg.twist.linear.y = y; + msg.twist.angular.y = y; + msg.twist.linear.z = alt; + msg.twist.angular.z = yaw; + // ROS_INFO("set_velocity: x:%f, y:%f, alt:%f, yaw:%f",x,y,alt,yaw); + pre_location=gps_object.gps_position(); + ROS_INFO("%f,%f",pre_location.lat,pre_location.lon); + + uint64_t last_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + int flag = 0; + while(true){ + velocity_command.publish(msg); + + uint64_t now_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + if((now_ms-last_ms)>second*1000){ + // ROS_INFO("%ld",now_ms-last_ms); + velocity_init(); + flag=1; + ROS_INFO("fin"); + break; + } + } + + while(flag==1){ + sleep(1); + if(fix_position(pre_location,x,y,second)!=0){ + ROS_INFO("position_fixed"); + break; + } + + } + +} + +void CommandClass::set_global_position(float lat,float lon,float alt){ + + mavros_msgs::GlobalPositionTarget goal_position; + goal_position.coordinate_frame = goal_position.FRAME_GLOBAL_TERRAIN_ALT; + goal_position.type_mask = goal_position.IGNORE_VX | goal_position.IGNORE_VY | goal_position.IGNORE_VZ | goal_position.IGNORE_AFX | goal_position.IGNORE_AFY | goal_position.IGNORE_AFZ | goal_position.IGNORE_YAW | goal_position.IGNORE_YAW_RATE; + goal_position.latitude = lat; + goal_position.longitude = lon; + goal_position.altitude = alt; + gps_command.publish(goal_position); + +} + +int CommandClass::fix_position(global_location pre_location,float x,float y,float s){ + + current_location=gps_object.gps_position(); + float offset_lon=((pre_location.lon-100)*1e+5)+(x*s)-((current_location.lon-100)*1e+5); + float offset_lat=(pre_location.lat*1e+5)+(y*s)-(current_location.lat*1e+5); + // ROS_INFO("pre(%f,%f)",(pre_location.lon-100)*1e+5,(pre_location.lat*1e+5)); + // ROS_INFO("cur(%f,%f)",((current_location.lon-100)*1e+5),(current_location.lat*1e+5)); + ROS_INFO("of_lon:%f,of_lat:%f",offset_lon,offset_lat); + if(offset_lon>1 || offset_lat>1 || offset_lon<-1 || offset_lat<-1){ + fix_velocity(offset_lon/10,offset_lat/10,0,0,1); + // sleep(1); + return 0; + } + else{ + return 1; + } +} +void CommandClass::set_velocity(float x,float y,float alt,float yaw,float second){ + + msg.twist.linear.x = x; + msg.twist.angular.x = x; + msg.twist.linear.y = y; + msg.twist.angular.y = y; + msg.twist.linear.z = alt; + msg.twist.angular.z = yaw; + // ROS_INFO("set_velocity: x:%f, y:%f, alt:%f, yaw:%f",x,y,alt,yaw); + global_location pre_location=gps_object.gps_position(); + + uint64_t last_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + + while(true){ + velocity_command.publish(msg); + + uint64_t now_ms = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + if((now_ms-last_ms)>second*1000){ + // ROS_INFO("%ld",now_ms-last_ms); + //velocity_init(); + break; + } + } +} + +void CommandClass::set_target_position(float x ,float y ){ + + origin_location = gps_object.gps_position(); + target_location.lon = (origin_location.lon*1e+5)+x; + target_location.lat = (origin_location.lat*1e+5)+y; + float PID[]={0.25,0.1,0}; + + while(true){ + + current_location = gps_object.gps_position(); + errorX = (current_location.lon-origin_location.lon)*1e+5; + errorY = (current_location.lat-origin_location.lat)*1e+5; + + lon_speed = clip(PID[0]*x + PID[1]*(x-errorX),1.5,-1.5); + lat_speed = clip(PID[0]*y + PID[1]*(y-errorY),1.5,-1.5); + ROS_INFO("%f,%f",lon_speed,lat_speed); + + + if(sqrt(pow(errorX,2)+pow(errorY,2)) < (sqrt(x*x+y*y)-0.5)){ + fix_velocity(lon_speed,lat_speed,0,0,0.2); + } + else if(fix_position(origin_location,x,y,1) != 0){ + break; + } + } + +} + +float CommandClass::clip(float speed,float max_speed,float min_speed){ + + if (speed > 0){ + if(speed > max_speed){ + speed = max_speed; + } + // if(speed < 0.5){ + // speed = 0.5; + // } + return speed; + } + else if(speed < 0){ + if(speed < min_speed){ + speed = min_speed; + } + // if(speed > -0.5){ + // speed = -0.5; + // } + return speed; + } + else{ + return speed; + } + +} \ No newline at end of file diff --git a/class_model/src/command.py b/class_model/src/command.py new file mode 100755 index 0000000..ca73e08 --- /dev/null +++ b/class_model/src/command.py @@ -0,0 +1,77 @@ +#! /usr/bin/env python3 +#coding:utf-8 + +import ssl +import rospy +from std_msgs.msg import String +import paho.mqtt.client as mqtt +import json + + + # Ros +def ros_pub(dataJson): + global publisher, rate ,publisher1 ,publisher2,publisher3,publisher4,publisher5 + # data = json.loads(dataJson) + publisher.publish(dataJson) #將date字串發布到topic + publisher1.publish(dataJson) + publisher2.publish(dataJson) + publisher3.publish(dataJson) + publisher4.publish(dataJson) + publisher5.publish(dataJson) + + rate.sleep() + # print(f"publish data {data}") + +# MQTT +def on_connect(self, userdata, flags, rc): + print("Connected with result code " + str(rc)) + client.subscribe("cmd/broadcast") + + +def on_message(self, userdata, msg): + #msg = msg.payload.decode('utf-8') + print(f"msg.topic {msg}") + ros_pub(msg.payload.decode('utf-8')) + + +def initialise_clients(clientName): + # callback assignment + initialise_client = mqtt.Client(clientName, False) + initialise_client.topic_ack = [] + return initialise_client + + +if __name__ == '__main__': + # Mqtt + mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "cmd/broadcast"} + client = initialise_clients("receiver") + client.on_connect = on_connect + client.on_message = on_message + client.connect(mqtt_config["host"], mqtt_config["port"], 60) + + # Ros + Mqtt_Node = 'publisher_py' + rospy.init_node("cmd_receiver") + # initialize Ros node + topicName = 'cmd_receiver' + publisher = rospy.Publisher(topicName,String,queue_size=10) + + topicName1 = '/drone1/cmd_receiver' + publisher1 = rospy.Publisher(topicName1,String,queue_size=10) + + topicName2 = '/drone2/cmd_receiver' + publisher2 = rospy.Publisher(topicName2,String,queue_size=10) + + topicName3 = '/drone3/cmd_receiver' + publisher3 = rospy.Publisher(topicName3,String,queue_size=10) + + topicName4 = '/drone4/cmd_receiver' + publisher4 = rospy.Publisher(topicName4,String,queue_size=10) + + topicName5 = '/drone5/cmd_receiver' + publisher5 = rospy.Publisher(topicName5,String,queue_size=10) + + + rate = rospy.Rate(10) + + client.loop_forever() diff --git a/class_model/src/control.cpp b/class_model/src/control.cpp new file mode 100755 index 0000000..383455b --- /dev/null +++ b/class_model/src/control.cpp @@ -0,0 +1,60 @@ +#include"class_model/control.h" + +ControlClass::ControlClass() : node_handle_("~"){ + + std::string ros_namespace; + if (!node_handle_.hasParam("namespace")) + { + }else{ + node_handle_.getParam("namespace", ros_namespace); + } + arming_client=node_handle_.serviceClient(ros_namespace+"/mavros/cmd/arming"); + takeoff_client=node_handle_.serviceClient(ros_namespace+"/mavros/cmd/takeoff"); + state_sub = node_handle_.subscribe(ros_namespace+"/mavros/state", 10, &ControlClass::state_cb,this); +} + +ControlClass::~ControlClass() { ros::shutdown(); } + + +void ControlClass::state_cb(const mavros_msgs::State::ConstPtr& msg){ + + current_state_g = *msg; + +} + +int ControlClass::arm() { + + ROS_INFO("Arming drone"); + mavros_msgs::CommandBool arm_request; + arm_request.request.value = true; + while (!current_state_g.armed && !arm_request.response.success && ros::ok()) + { + ros::Duration(.1).sleep(); + arming_client.call(arm_request); + } + if(arm_request.response.success) + { + ROS_INFO("Arming Successful"); + return 0; + }else{ + ROS_INFO("Arming failed with %d", arm_request.response.success); + return -1; + } + +} + +int ControlClass::takeoff(float takeoff_alt) { + + mavros_msgs::CommandTOL srv_takeoff; + srv_takeoff.request.altitude = takeoff_alt; + if(takeoff_client.call(srv_takeoff)){ + sleep(3); + ROS_INFO("takeoff sent %d", srv_takeoff.response.success); + }else{ + ROS_ERROR("Failed Takeoff"); + return -2; + } + sleep(2); + return 0; + +} \ No newline at end of file diff --git a/class_model/src/drone2.cpp b/class_model/src/drone2.cpp new file mode 100755 index 0000000..dcc84de --- /dev/null +++ b/class_model/src/drone2.cpp @@ -0,0 +1,19 @@ +#include "class_model/follower.h" + +int main(int argc, char **argv) { + + // Init ROS node + ros::init(argc, argv, "drone2_node"); + + // reate Assync spiner + ros::AsyncSpinner spinner(0); + spinner.start(); + + FollowerClass follower_object; + follower_object.follower(); + // RequestClass test_object; + + ros::waitForShutdown(); + + return 0; +} diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp new file mode 100755 index 0000000..0c695f5 --- /dev/null +++ b/class_model/src/follower.cpp @@ -0,0 +1,64 @@ +#include "class_model/follower.h" + +FollowerClass::FollowerClass() : node_handle_(""){ + + +} + +FollowerClass::~FollowerClass() { ros::shutdown(); } + +void FollowerClass::follower(){ + + mode_object.set_Mode("GUIDED"); + control_object.arm(); + control_object.takeoff(1.5); + sleep(5); + while(true){ + calculate_position(4,30); + } + + sleep(2); + + mode_object.set_Mode("LAND"); + +} + +void FollowerClass::calculate_position(float k,float theta){ + + theta = theta*3.14/180; + float phi = request_object.get_leader_heading()/100; + phi = phi*3.14/180; + leader_location=request_object.get_leader_GPS(); + follower_location=GPS_object.gps_position(); + + double Pf[]={},Pl[]={leader_location.lon,leader_location.lat}; + float transfer[2][2]={ + cos(phi),-sin(phi), + sin(phi),cos(phi) + }; + float Q[2]={k*sin(theta),k*cos(theta)}; + float T[2]={1,-1}; + + Pf[0]=(transfer[0][0]*Q[0]+transfer[0][1]*Q[1])*T[0]+Pl[0]/100; + Pf[1]=(transfer[1][0]*Q[0]+transfer[1][1]*Q[1])*T[1]+Pl[1]/100; + + float error_x = Pf[0] - (follower_location.lon*1e+5); + float error_y = Pf[1] - (follower_location.lat*1e+5); + + if (error_x < 0.3 & error_x > -0.3){ + error_x = 0; + } + if (error_y < 0.3 & error_y > -0.3){ + error_y = 0; + } + + // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100); + ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5); + ROS_INFO("%f,%f",Pf[0],Pf[1]); + ROS_INFO("%f,%f",error_x,error_y); + ROS_INFO("************************************"); + + command_object.fix_velocity(error_x,error_y,0,0,0.1); + // sleep(0.5); + +} diff --git a/class_model/src/formation.cpp b/class_model/src/formation.cpp new file mode 100755 index 0000000..e2cc4cf --- /dev/null +++ b/class_model/src/formation.cpp @@ -0,0 +1,604 @@ +#include "class_model/formation.h" +#include "class_model/convert_degree.h" + +FormationClass::FormationClass() : node_handle_(""){ + + std::string ros_namespace; + if (!node_handle_.hasParam("namespace")) + { + }else{ + node_handle_.getParam("namespace", ros_namespace); + } + + next_command=node_handle_.advertise(ros_namespace+"/mavros/next_command",100); + + + +} + +FormationClass::~FormationClass() { ros::shutdown(); } + +void FormationClass::leader(){ + + int counter=0; + std::string command = "",pre_command = ""; + + while(true){ + if(flag==0){ + command_object.set_velocity(0,0,0,0.1,1); + sleep(5); + while(counter<=5){ + command_object.set_velocity(-1,1,0,0,1); + 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; + } + + } + + counter++; + std::cout << command <=5){ + flag = 1; + } + command = receiver_object.check_command(); + // command_object.set_velocity(-1,1,0,0,10); + // // sleep(5); + // command_object.set_velocity(0,0,0,0,10); + // command_object.set_velocity(1,-1,0,0,10); + // command_object.set_velocity(1,-1,0,0,10); + // sleep(2); + } + if(command == "land"){ + mode_object.set_Mode("LAND"); + ROS_INFO("xxxx"); + } + +} +} + + +void FormationClass::follower1(int type){ + + std::string command,pre_command = ""; + while(true){ + + if(type == 0){ + calculate_position(4,30); + }else if(type == 1){ + calculate_position(4,0); + }else if(type == 2){ + calculate_position(4,90); + }else if(type == 3){ + calculate_position(4,120); + }else if(type == 4){ + calculate_position(2.5,120); + }else if(type == 5){ + calculate_position(4,60,60); + } + + command = receiver_object.check_command(); + if(command != pre_command){ + // ROS_INFO("change formation"); + break; + } + pre_command = command; + + + } + + // sleep(2); + + // mode_object.set_Mode("LAND"); + +} + +void FormationClass::follower2(int type){ + + std::string command,pre_command = ""; + while(true){ + + if(type == 0){ + calculate_position(4,-30); + }else if(type == 1){ + calculate_position(8,0); + }else if(type == 2){ + calculate_position(4,-90); + }else if(type == 3){ + calculate_position(4,-120); + }else if(type == 4){ + calculate_position(2.5,-120); + }else if(type == 5){ + calculate_position(4,-60,-60); + } + + command = receiver_object.check_command(); + if(command != pre_command){ + // ROS_INFO("change formation"); + break; + } + pre_command = command; + + + } + +} + +void FormationClass::follower3(int type){ + + std::string command,pre_command = ""; + while(true){ + + if(type == 0){ + calculate_position(8,30); + }else if(type == 1){ + calculate_position(12,0); + }else if(type == 2){ + calculate_position(8,90); + }else if(type == 3){ + calculate_position(6,160); + }else if(type == 4){ + calculate_position(2.5,60); + }else if(type == 5){ + calculate_position(6.93,30,120); + } + + + command = receiver_object.check_command(); + if(command != pre_command){ + // ROS_INFO("change formation"); + break; + } + pre_command = command; + + + } + +} + +void FormationClass::follower4(int type){ + + std::string command,pre_command = ""; + while(true){ + + if(type == 0){ + calculate_position(8,-30); + }else if(type == 1){ + calculate_position(16,0); + }else if(type == 2){ + calculate_position(8,-90); + }else if(type == 3){ + calculate_position(6,-160); + }else if(type == 4){ + calculate_position(2.5,-60); + }else if(type == 5){ + calculate_position(6.93,-30,-120); + } + + command = receiver_object.check_command(); + if(command != pre_command){ + // ROS_INFO("change formation"); + break; + } + pre_command = command; + + + } + +} + +void FormationClass::follower5(int type){ + + std::string command,pre_command = ""; + while(true){ + + if(type == 0){ + calculate_position(12,-30); + }else if(type == 1){ + calculate_position(20,0); + }else if(type == 2){ + calculate_position(6,-90); + }else if(type == 3){ + calculate_position(6,180); + }else if(type == 4){ + calculate_position(3,0); + }else if(type == 5){ + calculate_position(8,0,180); + } + + command = receiver_object.check_command(); + if(command != pre_command){ + // ROS_INFO("change formation"); + break; + } + pre_command = command; + + + } + +} + +void FormationClass::sph_follower1(int type){ + + std::string command,pre_command = ""; + while(true){ + + if(type == 0){ + spherical_coordinate(4,30,30); + // }else if(type == 1){ + // calculate_position(12,0,1); + // }else if(type == 2){ + // calculate_position(8,90,1); + // }else if(type == 3){ + // calculate_position(6,160,1); + // }else if(type == 4){ + // calculate_position(2.5,60,1); + } + + + command = receiver_object.check_command(); + if(command != pre_command){ + // ROS_INFO("change formation"); + break; + } + pre_command = command; + + + } + +} + +void FormationClass::sph_follower2(int type){ + + std::string command,pre_command = ""; + while(true){ + + if(type == 0){ + spherical_coordinate(4,-30,30); + // }else if(type == 1){ + // calculate_position(12,0,1); + // }else if(type == 2){ + // calculate_position(8,90,1); + // }else if(type == 3){ + // calculate_position(6,160,1); + // }else if(type == 4){ + // calculate_position(2.5,60,1); + } + + + command = receiver_object.check_command(); + if(command != pre_command){ + // ROS_INFO("change formation"); + break; + } + pre_command = command; + + + } + +} + +void FormationClass::sph_follower3(int type){ + + std::string command,pre_command = ""; + while(true){ + + if(type == 0){ + spherical_coordinate(4,30,-30); + // }else if(type == 1){ + // calculate_position(12,0,1); + // }else if(type == 2){ + // calculate_position(8,90,1); + // }else if(type == 3){ + // calculate_position(6,160,1); + // }else if(type == 4){ + // calculate_position(2.5,60,1); + } + + + command = receiver_object.check_command(); + if(command != pre_command){ + // ROS_INFO("change formation"); + break; + } + pre_command = command; + + + } +} + +void FormationClass::sph_follower4(int type){ + + std::string command,pre_command = ""; + while(true){ + + if(type == 0){ + spherical_coordinate(4,-30,-30); + // }else if(type == 1){ + // calculate_position(12,0,1); + // }else if(type == 2){ + // calculate_position(8,90,1); + // }else if(type == 3){ + // calculate_position(6,160,1); + // }else if(type == 4){ + // calculate_position(2.5,60,1); + } + + + command = receiver_object.check_command(); + if(command != pre_command){ + // ROS_INFO("change formation"); + break; + } + pre_command = command; + + + } +} + + +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){ + + float deg_phi = request_object.get_leader_heading()/100; + float heading = GPS_object.get_heading(); + + float phi = deg_phi*3.14/180; //degree-->radian + theta = theta*3.14/180; + + leader_location=request_object.get_leader_GPS(); //get Leader/Follower 's GPS + follower_location=GPS_object.gps_position(); + + + double Pf[]={},Pl[]={leader_location.lon,leader_location.lat}; //transfer maxtrix + float transfer[2][2]={ + cos(phi),-sin(phi), + sin(phi),cos(phi) + }; + float Q[2]={k*sin(theta),k*cos(theta)}; + float T[2]={1,-1}; + + Pf[0]=(transfer[0][0]*Q[0]+transfer[0][1]*Q[1])*T[0]+Pl[0]/100; + Pf[1]=(transfer[1][0]*Q[0]+transfer[1][1]*Q[1])*T[1]+Pl[1]/100; + + + float error_x = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid); //caculate error + float error_y = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid); + + // float error_x = PID_x.update1(follower_location.lon*1e5 , Pf[0] ); //caculate error + // float error_y = PID_y.update1(follower_location.lat*1e5 , Pf[1] ); + + // float error_x = Pf[0] - (follower_location.lon*1e+5); //caculate error + // float error_y = Pf[1] - (follower_location.lat*1e+5); + + float error_degree = deg_phi - heading + direction; + float error_yaw = 0.2; //CCW + + if (error_degree >= 360){ + error_degree -= 360; + } + if (error_degree <= -360){ + error_degree += 360; + } + + error_yaw = check_direction(error_degree)*error_yaw; //check yaw direction + + if (error_x < 0.3 && error_x > -0.3){ //ignore small errors + error_x = 0; + } + if (error_y < 0.3 && error_y > -0.3){ + error_y = 0; + } + if (error_degree < 5 && error_degree > -5 ){ + error_yaw = 0; + }else if(error_degree >355 || error_degree < -355){ + error_yaw = 0; + } + + // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100); + // ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5); + // ROS_INFO("%f,%f",Pf[0],Pf[1]); + // ROS_INFO("%f,%f",error_x,error_y); + // ROS_INFO("deg:%f",deg_phi); + // ROS_INFO("heading:%f",heading); + // ROS_INFO("error_degree:%f",error_degree); + // ROS_INFO("error_yaw:%f",error_yaw); + // ROS_INFO("************************************"); + + command_object.set_velocity(error_x,error_y,0,error_yaw,0.1); + +} + +void FormationClass::spherical_coordinate(float k,float theta,float psi){ + + float deg_phi = request_object.get_leader_heading()/100; + float heading = GPS_object.get_heading(); + + float phi = deg_phi*3.14/180; //degree-->radian + theta = theta*3.14/180; + psi = psi*3.14/180; + + leader_location=request_object.get_leader_GPS(); //get Leader/Follower 's GPS + follower_location=GPS_object.gps_position(); + + cur_alt = leader_location.alt; + if (cur_alt == NAN || cur_alt == -NAN){ + cur_alt = pre_alt; + } + + double Pf[3]={},Pl[]={leader_location.lon,leader_location.lat,cur_alt}; //transfer maxtrix + float transfer[3][3]={ + cos(phi),-sin(phi),0, + sin(phi),cos(phi),0, + 0 , 0 ,1, + }; + float Q[3]={k*cos(psi)*sin(theta),k*cos(psi)*cos(theta),k*sin(psi)}; + float T[3]={1,-1,1}; + + Pf[0]=(transfer[0][0]*Q[0] + transfer[0][1]*Q[1] + transfer[0][2]*Q[2])*T[0] + Pl[0]/100; //calculate follower coordinate + Pf[1]=(transfer[1][0]*Q[0] + transfer[1][1]*Q[1] + transfer[1][2]*Q[2])*T[1] + Pl[1]/100; //rigid body ---> world + Pf[2]=(transfer[2][2]*Q[2])*T[2] + Pl[2]/100; + + if(Pf[2] == NAN || Pf[2] == -NAN){ + Pf[2] = k*sin(psi) + cur_alt/100; + } + + float error_x = PID_x.update(follower_location.lon*1e5 , Pf[0] ,follower_pid); //caculate error + float error_y = PID_y.update(follower_location.lat*1e5 , Pf[1] ,follower_pid); //follower_pid defined in header file + float error_alt = Pf[2] - follower_location.alt; + + // float error_x = PID_x.update1(follower_location.lon*1e5 , Pf[0] ); //caculate error + // float error_y = PID_y.update1(follower_location.lat*1e5 , Pf[1] ); + + // float error_x = Pf[0] - (follower_location.lon*1e+5); //caculate error + // float error_y = Pf[1] - (follower_location.lat*1e+5); + + float error_degree = deg_phi - heading; + float error_yaw = 0.2; + float error_z = (error_alt/std::abs(error_alt))*0.1; + + error_yaw = check_direction(error_degree)*error_yaw; //check yaw direction + + if (error_x < 0.3 && error_x > -0.3){ //ignore small errors + error_x = 0; + } + if (error_y < 0.3 && error_y > -0.3){ + error_y = 0; + } + if (error_degree < 5 && error_degree > -5 ){ + error_yaw = 0; + }else if(error_degree >355 || error_degree < -355){ + error_yaw = 0; + } + if(error_alt < 0.3 && error_alt > -0.3){ + error_z = 0; + } + + pre_alt = leader_location.alt; + + // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100); + // ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5); + // ROS_INFO("%f,%f",Pf[0],Pf[1]); + // ROS_INFO("%f,%f",error_x,error_y); + // ROS_INFO("deg:%f",deg_phi); + // ROS_INFO("heading:%f",heading); + // ROS_INFO("error_degree:%f",error_degree); + // ROS_INFO("error_yaw:%f",error_yaw); + ROS_INFO("Pl[2]:%f ,error_alt:%f,follower_location.alt:%f",Pl[2],error_alt,follower_location.alt); + ROS_INFO("Q[2]:%f ,Pf[2]:%f ,error_z:%f",k*sin(psi),Pf[2],error_z); + // ROS_INFO("************************************"); + + command_object.set_velocity(error_x,error_y,error_z,error_yaw,0.1); + +} + +void FormationClass::go2target(float x,float y){ + + leader_location=request_object.get_leader_GPS(); + current_location=GPS_object.gps_position(); + + float target_heading,error_heading; + float heading = GPS_object.get_heading(); + target_heading = ConvertDeg(x,y); + + // float error_yaw = 0.2; + // error_heading = target_heading - heading; + // error_yaw = check_direction(error_heading)*error_yaw; //check yaw direction + + while(heading_status != 1){ + 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); + + // 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 (error_y < 0.3 && error_y > -0.3){ + error_y = 0; + } + + if (error_x == 0 && error_y == 0){ + flag = 1; + } + + command_object.set_velocity(error_x ,error_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); + // ROS_INFO("target:%f,%f",target_location.lon ,target_location.lat); + // ROS_INFO("current:%f,%f",current_location.lon*1e5 ,current_location.lat*1e5); + ROS_INFO("************************************"); + + + +} + +void FormationClass::face2target(float target_heading){ + + float error_heading; + float heading = GPS_object.get_heading(); + float error_yaw = 0.2; + error_heading = target_heading - heading; + error_yaw = check_direction(error_heading)*error_yaw; + + if (error_heading < 5 && error_heading > -5 ){ + error_yaw = 0; + heading_status = 1; + } + command_object.set_velocity(0 ,0 ,0,error_yaw,0.1); +} + + + diff --git a/class_model/src/getParam.cpp b/class_model/src/getParam.cpp new file mode 100755 index 0000000..392d6f6 --- /dev/null +++ b/class_model/src/getParam.cpp @@ -0,0 +1,24 @@ +#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; + } + + +} \ No newline at end of file diff --git a/class_model/src/local_mqtt_pub_data_to_ros.py b/class_model/src/local_mqtt_pub_data_to_ros.py new file mode 100755 index 0000000..d6f8798 --- /dev/null +++ b/class_model/src/local_mqtt_pub_data_to_ros.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 +#coding:utf-8 +# license removed for brevity +import ssl +import rospy +from std_msgs.msg import String +import paho.mqtt.client as mqtt +import json + + +# Ros +def ros_pub(dataJson): + global publisher, rate + # data = json.loads(dataJson) + publisher.publish(dataJson) #將date字串發布到topic + rate.sleep() + # print(f"publish data {data}") + +# MQTT +def initialise_clients(clientname): + # callback assignment + initialise_client = mqtt.Client(clientname, False) + initialise_client.topic_ack = [] + return initialise_client + +def on_connect(client, userdata, flags, rc): + print("Connected with result code " + str(rc)) + # client.subscribe(mqtt_config["topic"]) + client.subscribe("data/imu") + + +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')) + + +if __name__ == '__main__': + # Mqtt + mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/imu"} + client = initialise_clients("123456") + client.on_connect = on_connect + client.on_message = on_message + client.connect(mqtt_config["host"], mqtt_config["port"], 60) + + + + # Ros + Mqtt_Node = 'publisher_py' + rospy.init_node(Mqtt_Node) + # initialize Ros node + topicName = 'uav_message' + publisher = rospy.Publisher(topicName,String,queue_size=10) + rate = rospy.Rate(10) + + + + client.loop_forever() + +# mqtt connect code list +# 0: Connection successful +# 1: Connection refused – incorrect protocol version +# 2: Connection refused – invalid client identifier +# 3: Connection refused – server unavailable +# 4: Connection refused – bad username or password +# 5: Connection refused – not authorised +# 6-255: Currently unused. diff --git a/class_model/src/local_mqtt_sub_data_from_ros.py b/class_model/src/local_mqtt_sub_data_from_ros.py new file mode 100755 index 0000000..b35f8f5 --- /dev/null +++ b/class_model/src/local_mqtt_sub_data_from_ros.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python3 +#coding:utf-8 +import sys +import json +import paho.mqtt.client as mqtt + + +from traceback import print_tb +import rospy +from std_msgs.msg import String +from std_msgs.msg import Float64 +from std_msgs.msg import Header +from mavros_msgs.srv import ParamGet +from sensor_msgs.msg import NavSatFix +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"} +data ={} +# Ros +def callBack_imu(IMU): + gyro_x=str(IMU.linear_acceleration.x) + gyro_y=str(IMU.linear_acceleration.y) + gyro_z=str(IMU.linear_acceleration.z) + + accel_x=str(IMU.angular_velocity.x) + accel_y=str(IMU.angular_velocity.y) + accel_z=str(IMU.angular_velocity.z) + + dataImuUpdate = {"gyro_x": gyro_x, "gyro_y": gyro_y,"gyro_z":gyro_z, "accel_x": accel_x, "accel_y": accel_y, "accel_z": accel_z} + data.update(dataImuUpdate) + # print ('gyro_x:'+gyro_x+'\n'+'gyro_y:'+gyro_y+'\n'+'gyro_z:'+gyro_z +'\n') + # print ('accel_x:'+accel_x+'\n'+'accel_y:'+accel_y+'\n'+'accel_z:'+accel_z +'\n') + + +def callBack_gps(GPS): + lat=str(int(GPS.latitude*10000000)) #change the scale to centimeters + lon=str(int(GPS.longitude*10000000)) + alt=str(int(GPS.altitude*100)) + dataGpsUpdate = {"lat": lat, "lon": lon, "alt": alt} + data.update(dataGpsUpdate) + # dataJsonFormate = json.dumps(data) + # mqtt_Pub(message=dataJsonFormate) + # print ('lat:'+lat+'\n'+'lon:'+lon+'\n') + + +def callBack_compass_hdg(Compass): + heading = str(int(Compass.data*100)) + dataGpsUpdate = {"heading": heading} + data.update(dataGpsUpdate) + dataJsonFormate = json.dumps(data) + mqtt_Pub(message=dataJsonFormate) + +def callBack_state(state): + mode = state.mode + dataGpsUpdate = {"mode": mode} + data.update(dataGpsUpdate) + +def GetParam(self,param_name): + param = self.get_param_srv(param_name) + if param.success: + if param.value.integer != 0: + value = param.value.integer + + else: + value = param.value.real + + else: + rospy.logwarn("Parameter "+param_name+" not read") + value = 0 + return value + + +# MQTT +def initialise_clients(clientname): + # callback assignment + initialise_client = mqtt.Client(clientname, False) + initialise_client.topic_ack = [] + return initialise_client + + +# publish a message +def mqtt_Pub(message, topics = mqtt_config["topic"], 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) + +def on_connect(self, userdata, flags, rc): + print("Connected with result code " + str(rc)) + + +if __name__ == '__main__': + # Mqtt + client = initialise_clients("client1") + client.on_publish = on_publish + client.on_connect = on_connect + client.connect(mqtt_config["host"], mqtt_config["port"], 60) + client.loop_start() + + # Ros + nodeName = 'save_data_py' + rospy.init_node(nodeName) + + ros_namespace = "" + if not rospy.has_param("namespace"): + print("using default namespace") + else: + rospy.get_param("namespace", ros_namespace) + print("using namespace "+ros_namespace) + + ros_namespace="/drone1" + # topicName = 'data_topic' + # 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/mavros_msgs/State',Header,callBack_state) + ID = rospy.get_param(ros_namespace+'/leader/droneID') + # subscriber = rospy.Subscriber('/mavros/rangefinder/rangefinder',Range,callBack_rng) #從topic獲取string再呼叫callback + + # ID = rospy.GetParam("droneID") + # print(ID) + + rospy.spin() \ No newline at end of file diff --git a/class_model/src/main.cpp b/class_model/src/main.cpp new file mode 100755 index 0000000..f53e4d4 --- /dev/null +++ b/class_model/src/main.cpp @@ -0,0 +1,72 @@ +#include "class_model/mission.h" + +int main(int argc, char **argv) { + + // Init ROS node + ros::init(argc, argv, "drone1_node"); + + // reate Assync spiner + ros::AsyncSpinner spinner(0); + spinner.start(); + + 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"); + control_object.arm(); + control_object.takeoff(4.5); + sleep(5); + + while(ros::ok()){ + + 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"); + } + + + //mission_object.cruise(20,20); + // select_formation.goose_formation(); + // }else if (type == "hex"){ + // select_formation.Hex_formation(); + // } + // select_formation.Hex_formation(); + sleep(0.5); + // command_object.set_velocity(0 ,0 ,0,0,1); + + if(type == "stop"){ + select_formation.stop(); + }else if(type == "land"){ + mode_object.set_Mode("LAND"); + } + } + + + // select_formation.square(); + + // RequestClass test_object; + + ros::waitForShutdown(); + + return 0; +} diff --git a/class_model/src/mission.cpp b/class_model/src/mission.cpp new file mode 100755 index 0000000..2f3adab --- /dev/null +++ b/class_model/src/mission.cpp @@ -0,0 +1,50 @@ +#include "class_model/mission.h" + + +MissionClass::MissionClass() : node_handle_(""){ + + +} + +MissionClass::~MissionClass() { ros::shutdown(); } + +void MissionClass::fly2target(float x,float y){ + select_formation.protect_formation(x,y); + sleep(1); + select_formation.protect_formation(-x,-y); + // select_formation.stop(); + ROS_INFO("-----------------------"); +} + +void MissionClass::cruise(float x, float y){ + + // select_formation.protect_formation(0,y); + // sleep(1); + // select_formation.protect_formation(-x,0); + // sleep(1); + // select_formation.protect_formation(0,-y); + // sleep(1); + // select_formation.protect_formation(x,0); + // sleep(1); + + select_formation.init_formation(0,y); + sleep(1); + select_formation.init_formation(-x,0); + sleep(1); + select_formation.init_formation(0,-y); + sleep(1); + select_formation.init_formation(x,0); + sleep(1); +} + +void MissionClass::snake(float x, float y){ + + select_formation.line_formation(0,y); + sleep(1); + select_formation.line_formation(-x,0); + sleep(1); + select_formation.line_formation(0,-y); + sleep(1); + select_formation.line_formation(x,0); + sleep(1); +} \ No newline at end of file diff --git a/class_model/src/mode.cpp b/class_model/src/mode.cpp new file mode 100755 index 0000000..2ca8e80 --- /dev/null +++ b/class_model/src/mode.cpp @@ -0,0 +1,33 @@ +#include"class_model/mode.h" + +ModeClass::ModeClass() : node_handle_("~"){ + + std::string ros_namespace; + if (!node_handle_.hasParam("namespace")) + { + ROS_INFO("using default namespace"); + }else{ + node_handle_.getParam("namespace", ros_namespace); + ROS_INFO("using namespace %s", ros_namespace.c_str()); + } + + set_mode_client=node_handle_.serviceClient(ros_namespace+"/mavros/set_mode"); + +} + +ModeClass::~ModeClass() { ros::shutdown(); } + +int ModeClass::set_Mode(std::string mode) { + + mavros_msgs::SetMode srv_setMode; + srv_setMode.request.base_mode = 0; + srv_setMode.request.custom_mode = mode.c_str(); + if(set_mode_client.call(srv_setMode)){ + // ROS_INFO("setmode %s ok",mode.c_str()); + return 0; + }else{ + ROS_ERROR("Failed SetMode %s",mode.c_str()); + return -1; + } + +} \ No newline at end of file diff --git a/class_model/src/receiver.cpp b/class_model/src/receiver.cpp new file mode 100755 index 0000000..a802a44 --- /dev/null +++ b/class_model/src/receiver.cpp @@ -0,0 +1,26 @@ +#include"class_model/receiver.h" + +ReceiverClass::ReceiverClass() : node_handle_("~"){ + + std::string ros_namespace; + if (!node_handle_.hasParam("namespace")) + { + }else{ + node_handle_.getParam("namespace", ros_namespace); + } + mqtt_sub = node_handle_.subscribe(ros_namespace+"/cmd_receiver",10,&ReceiverClass::cmd_receiver, this); +} + +ReceiverClass::~ReceiverClass() { ros::shutdown(); } + +void ReceiverClass::cmd_receiver(const std_msgs::String::ConstPtr &msg){ + + mqtt_command=""; + mqtt_command.append(msg->data); + // std::cout<< "type:" << msg->data << std::endl; +} + +std::string ReceiverClass::check_command(){ + // ROS_INFO("sd:%s",mqtt_command); + return mqtt_command; +} \ No newline at end of file diff --git a/class_model/src/requestData.cpp b/class_model/src/requestData.cpp new file mode 100755 index 0000000..43370f1 --- /dev/null +++ b/class_model/src/requestData.cpp @@ -0,0 +1,75 @@ +#include"class_model/requestData.h" +#include +#include + +global_location leader_position; + +RequestClass::RequestClass() : node_handle_(""){ + + mqtt_data = node_handle_.subscribe("/uav_message", 10, + &RequestClass::Data_callback, this); + +} + +RequestClass::~RequestClass() { ros::shutdown(); } + +void RequestClass::Data_callback(const std_msgs::String::ConstPtr &sensor) { + + std::string data = sensor->data; + jsonToInt(data); + +} + +global_location RequestClass::get_leader_GPS(){ + + return leader_position; +} + +float RequestClass::get_leader_heading(){ + + return heading; +} + +void RequestClass::jsonToInt(std::string data){ + + std::string lat,lon,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;i3){ + 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< + +global_location position; +float heading; + + +ThreadGPSClass::ThreadGPSClass() : node_handle_("~"){ + + std::string ros_namespace; + if (!node_handle_.hasParam("namespace")) + { + }else{ + node_handle_.getParam("namespace", ros_namespace); + } + + gps_sub = node_handle_.subscribe(ros_namespace+"/mavros/global_position/global", 10, + &ThreadGPSClass::GPS_callback, this); + compass_sub = node_handle_.subscribe(ros_namespace+"/mavros/global_position/compass_hdg", 10, + &ThreadGPSClass::Compass_callback, this); + +} + +ThreadGPSClass::~ThreadGPSClass() { ros::shutdown(); } + +void ThreadGPSClass::GPS_callback(const sensor_msgs::NavSatFix::ConstPtr &gps) { + + position.lat=gps->latitude; + position.lon=gps->longitude; + position.alt=gps->altitude; + // ROS_INFO("GPS [%f,%f]", position.lat, position.lon); +} + +void ThreadGPSClass::Compass_callback(const std_msgs::Float64::ConstPtr °ree){ + heading = degree->data; +} + +global_location ThreadGPSClass::gps_position(){ + // ROS_INFO("GPS [%f,%f]", position.lat, position.lon); + return position; +} + +float ThreadGPSClass::get_heading(){ + + return heading; +} \ No newline at end of file diff --git a/class_model/src/tower.py b/class_model/src/tower.py new file mode 100755 index 0000000..ef27e1f --- /dev/null +++ b/class_model/src/tower.py @@ -0,0 +1,59 @@ +#! /usr/bin/env python3 +#coding:utf-8 + +import paho.mqtt.client as mqtt +import time + + +def on_connect(self, userdata, flags, rc): + global connect_flag + print("Connected with result code " + str(rc)) + connect_flag = True + + +# def on_message(self, userdata, msg): +# print(f"Receive UAV_Z550 {msg.payload.decode('utf-8')}") +# print(f"Receive UAV_H380 {msg.payload.decode('utf-8')}") +# print("command: ", end="") + + +def initialise_clients(clientName): + # callback assignment + initialise_client = mqtt.Client(clientName, True) + initialise_client.topic_ack = [] + return initialise_client + + +# publish a message +def publish(topics, message, waitForAck=False): + mid = client.publish(topics, message, 1)[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) + + +connect_flag = False +mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "cmd/broadcast", "name": "Tower"} +client = initialise_clients(mqtt_config["name"]) +client.on_publish = on_publish +client.on_connect = on_connect +# client.on_message = on_message + +client.connect(mqtt_config["host"], mqtt_config["port"], 60) +client.loop_start() + +# publish(topicBroadcast, "Connect", True) +while True: + if connect_flag: + break + +while True: + command = input("command: ") + publish(mqtt_config["topic"], command)