first commit
commit
62bea117ee
@ -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
|
||||||
|
}
|
||||||
@ -0,0 +1,8 @@
|
|||||||
|
{
|
||||||
|
"python.autoComplete.extraPaths": [
|
||||||
|
"/opt/ros/noetic/lib/python3/dist-packages"
|
||||||
|
],
|
||||||
|
"python.analysis.extraPaths": [
|
||||||
|
"/opt/ros/noetic/lib/python3/dist-packages"
|
||||||
|
]
|
||||||
|
}
|
||||||
@ -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)
|
||||||
|
|
||||||
@ -0,0 +1,27 @@
|
|||||||
|
/*PID Controlier*/
|
||||||
|
#ifndef PID_H
|
||||||
|
#define PID_H
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include "ctime"
|
||||||
|
|
||||||
|
class PIDClass {
|
||||||
|
public:
|
||||||
|
// PIDClass();
|
||||||
|
// virtual ~PIDClass();
|
||||||
|
float update(float current ,float target ,float pidvals[3]);
|
||||||
|
float update1(float current ,float target );
|
||||||
|
private:
|
||||||
|
// ROS NodeHandle
|
||||||
|
ros::NodeHandle node_handle_;
|
||||||
|
|
||||||
|
float pidval1[3] = {0.5 , 0.000000000001 , 0.5};
|
||||||
|
float limit[2] = {2 , -2};
|
||||||
|
float targetvals,pError,I;
|
||||||
|
clock_t pTime = 0,current_time;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // PID_H
|
||||||
@ -0,0 +1,45 @@
|
|||||||
|
/*Drone Control Command*/
|
||||||
|
#ifndef COMMAND_H
|
||||||
|
#define COMMAND_H
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <geometry_msgs/TwistStamped.h>
|
||||||
|
#include <mavros_msgs/GlobalPositionTarget.h>
|
||||||
|
#include <class_model/sensor.h>
|
||||||
|
|
||||||
|
class CommandClass {
|
||||||
|
public:
|
||||||
|
CommandClass();
|
||||||
|
virtual ~CommandClass();
|
||||||
|
void set_global_position(float lat,float lon,float alt);
|
||||||
|
void set_velocity(float x,float y,float alt,float yaw,float second);
|
||||||
|
void fix_velocity(float x,float y,float alt,float yaw,float second);
|
||||||
|
void velocity_init();
|
||||||
|
void set_target_position(float x,float y);
|
||||||
|
ThreadGPSClass gps_object;
|
||||||
|
|
||||||
|
private:
|
||||||
|
// ROS NodeHandle
|
||||||
|
ros::NodeHandle node_handle_;
|
||||||
|
|
||||||
|
geometry_msgs::TwistStamped msg;
|
||||||
|
mavros_msgs::GlobalPositionTarget goal_position;
|
||||||
|
global_location current_location;
|
||||||
|
global_location target_location;
|
||||||
|
global_location origin_location;
|
||||||
|
global_location pre_location;
|
||||||
|
float errorX,errorY,lon_speed,lat_speed;
|
||||||
|
|
||||||
|
int fix_position(global_location pre_location,float x,float y,float s);
|
||||||
|
float clip(float speed,float max_speed,float min_speed);
|
||||||
|
//SERVICE
|
||||||
|
|
||||||
|
//SUBSCRIBE
|
||||||
|
|
||||||
|
//PUBLISHER
|
||||||
|
ros::Publisher velocity_command;
|
||||||
|
ros::Publisher gps_command;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // COMMAND_H
|
||||||
@ -0,0 +1,34 @@
|
|||||||
|
#ifndef CONTROL_H
|
||||||
|
#define CONTROL_H
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <mavros_msgs/CommandTOL.h>
|
||||||
|
#include <mavros_msgs/CommandBool.h>
|
||||||
|
#include <mavros_msgs/State.h>
|
||||||
|
|
||||||
|
class ControlClass {
|
||||||
|
public:
|
||||||
|
ControlClass();
|
||||||
|
virtual ~ControlClass();
|
||||||
|
int arm();
|
||||||
|
int takeoff(float takeoff_alt);
|
||||||
|
|
||||||
|
private:
|
||||||
|
// ROS NodeHandle
|
||||||
|
ros::NodeHandle node_handle_;
|
||||||
|
|
||||||
|
mavros_msgs::State current_state_g;
|
||||||
|
|
||||||
|
//SERVICE
|
||||||
|
ros::ServiceClient arming_client;
|
||||||
|
ros::ServiceClient takeoff_client;
|
||||||
|
|
||||||
|
//SUBSCRIBE
|
||||||
|
ros::Subscriber state_sub;
|
||||||
|
|
||||||
|
void state_cb(const mavros_msgs::State::ConstPtr& msg);
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // CONTROL_H
|
||||||
@ -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;
|
||||||
|
}
|
||||||
@ -0,0 +1,36 @@
|
|||||||
|
/*Follow the leader in a fixed formation */
|
||||||
|
#ifndef FOLLOWER_H
|
||||||
|
#define FOLLOWER_H
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include "class_model/sensor.h"
|
||||||
|
#include "class_model/mode.h"
|
||||||
|
#include "class_model/control.h"
|
||||||
|
#include "class_model/command.h"
|
||||||
|
#include "class_model/requestData.h"
|
||||||
|
|
||||||
|
|
||||||
|
class FollowerClass {
|
||||||
|
public:
|
||||||
|
FollowerClass();
|
||||||
|
virtual ~FollowerClass();
|
||||||
|
//ClassObject
|
||||||
|
ThreadGPSClass GPS_object;
|
||||||
|
ModeClass mode_object;
|
||||||
|
ControlClass control_object;
|
||||||
|
CommandClass command_object;
|
||||||
|
RequestClass request_object;
|
||||||
|
void follower();
|
||||||
|
private:
|
||||||
|
// ROS NodeHandle
|
||||||
|
ros::NodeHandle node_handle_;
|
||||||
|
|
||||||
|
global_location target_location;
|
||||||
|
global_location follower_location;
|
||||||
|
global_location leader_location;
|
||||||
|
|
||||||
|
void calculate_position(float k,float theta);
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // FOLLOWER_H
|
||||||
@ -0,0 +1,68 @@
|
|||||||
|
/*Follow the leader in a fixed formation */
|
||||||
|
#ifndef FORMATION_H
|
||||||
|
#define FORMATION_H
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include "class_model/sensor.h"
|
||||||
|
#include "class_model/mode.h"
|
||||||
|
#include "class_model/receiver.h"
|
||||||
|
#include "class_model/control.h"
|
||||||
|
#include "class_model/command.h"
|
||||||
|
#include "class_model/requestData.h"
|
||||||
|
#include "class_model/PID.h"
|
||||||
|
#include <std_msgs/String.h>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
class FormationClass {
|
||||||
|
public:
|
||||||
|
FormationClass();
|
||||||
|
virtual ~FormationClass();
|
||||||
|
//ClassObject
|
||||||
|
ThreadGPSClass GPS_object;
|
||||||
|
ModeClass mode_object;
|
||||||
|
ReceiverClass receiver_object;
|
||||||
|
ControlClass control_object;
|
||||||
|
CommandClass command_object;
|
||||||
|
RequestClass request_object;
|
||||||
|
PIDClass PID_x;
|
||||||
|
PIDClass PID_y;
|
||||||
|
|
||||||
|
void leader();
|
||||||
|
void leader1(float x=0.0,float y=0.0);
|
||||||
|
void follower1(int type);
|
||||||
|
void follower2(int type);
|
||||||
|
void follower3(int type);
|
||||||
|
void follower4(int type);
|
||||||
|
void follower5(int type);
|
||||||
|
void sph_follower1(int type);
|
||||||
|
void sph_follower2(int type);
|
||||||
|
void sph_follower3(int type);
|
||||||
|
void sph_follower4(int type);
|
||||||
|
void face2target(float target_heading);
|
||||||
|
private:
|
||||||
|
// ROS NodeHandle
|
||||||
|
ros::NodeHandle node_handle_;
|
||||||
|
|
||||||
|
global_location target_location;
|
||||||
|
global_location follower_location;
|
||||||
|
global_location leader_location;
|
||||||
|
global_location current_location;
|
||||||
|
int flag = 0,heading_status = 0;
|
||||||
|
float pre_alt,cur_alt;
|
||||||
|
float leader_pid[3] = {0.5 , 0.000000000001 ,0.5};
|
||||||
|
float follower_pid[3] = {1 , 0.00000000001 ,0.5};
|
||||||
|
|
||||||
|
void calculate_position(float k,float theta,int direction=0);
|
||||||
|
void spherical_coordinate(float k,float theta,float psi);
|
||||||
|
void go2target(float x,float y);
|
||||||
|
|
||||||
|
//PUBLISHER
|
||||||
|
ros::Publisher next_command;
|
||||||
|
|
||||||
|
// SUBSCRIBE
|
||||||
|
ros::Subscriber command_break;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // FORMATION_H
|
||||||
@ -0,0 +1,28 @@
|
|||||||
|
/*Get Drone's Parameter*/
|
||||||
|
#ifndef GETPARAM_H
|
||||||
|
#define GETPARAM_H
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
|
||||||
|
class ParamClass {
|
||||||
|
public:
|
||||||
|
ParamClass();
|
||||||
|
virtual ~ParamClass();
|
||||||
|
int getID();
|
||||||
|
|
||||||
|
private:
|
||||||
|
// ROS NodeHandle
|
||||||
|
ros::NodeHandle node_handle_;
|
||||||
|
|
||||||
|
//SERVICE
|
||||||
|
|
||||||
|
|
||||||
|
//SUBSCRIBE
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // GETPARAM_H
|
||||||
@ -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
|
||||||
@ -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
|
||||||
@ -0,0 +1,23 @@
|
|||||||
|
/*Setting Pixhawk Mode*/
|
||||||
|
#ifndef MODE_H
|
||||||
|
#define MODE_H
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <mavros_msgs/SetMode.h>
|
||||||
|
|
||||||
|
class ModeClass {
|
||||||
|
public:
|
||||||
|
ModeClass();
|
||||||
|
virtual ~ModeClass();
|
||||||
|
int set_Mode(std::string mode);
|
||||||
|
|
||||||
|
private:
|
||||||
|
// ROS NodeHandle
|
||||||
|
ros::NodeHandle node_handle_;
|
||||||
|
|
||||||
|
//SERVICE
|
||||||
|
ros::ServiceClient set_mode_client;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // MODE_H
|
||||||
@ -0,0 +1,28 @@
|
|||||||
|
/*Receive Command From MQTT*/
|
||||||
|
#ifndef RECEIVER_H
|
||||||
|
#define RECEIVER_H
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <std_msgs/String.h>
|
||||||
|
|
||||||
|
class ReceiverClass {
|
||||||
|
public:
|
||||||
|
ReceiverClass();
|
||||||
|
virtual ~ReceiverClass();
|
||||||
|
std::string check_command();
|
||||||
|
|
||||||
|
private:
|
||||||
|
// ROS NodeHandle
|
||||||
|
ros::NodeHandle node_handle_;
|
||||||
|
|
||||||
|
std::string mqtt_command;
|
||||||
|
|
||||||
|
//SERVICE
|
||||||
|
//SUBSCRIBE
|
||||||
|
ros::Subscriber mqtt_sub;
|
||||||
|
|
||||||
|
void cmd_receiver(const std_msgs::String::ConstPtr &msg);
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // RECEIVER_H
|
||||||
@ -0,0 +1,31 @@
|
|||||||
|
/*Subscribe Data Which MQTT Pubilsh */
|
||||||
|
#ifndef REQUESTDATA_H
|
||||||
|
#define REQUESTDATA_H
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <std_msgs/String.h>
|
||||||
|
#include <string>
|
||||||
|
#include <class_model/gps_location.h>
|
||||||
|
|
||||||
|
class RequestClass {
|
||||||
|
public:
|
||||||
|
RequestClass();
|
||||||
|
virtual ~RequestClass();
|
||||||
|
global_location get_leader_GPS();
|
||||||
|
float get_leader_heading();
|
||||||
|
|
||||||
|
private:
|
||||||
|
// ROS NodeHandle
|
||||||
|
ros::NodeHandle node_handle_;
|
||||||
|
|
||||||
|
void Data_callback(const std_msgs::String::ConstPtr &gps);
|
||||||
|
void jsonToInt(std::string data);
|
||||||
|
float heading;
|
||||||
|
// SUBSCRIBE
|
||||||
|
ros::Subscriber mqtt_data;
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // REQUESTDATA_H
|
||||||
|
|
||||||
@ -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
|
||||||
@ -0,0 +1,30 @@
|
|||||||
|
/*Subscribe Pixhawk Sensor Data*/
|
||||||
|
#ifndef SENSOR_H
|
||||||
|
#define SENSOR_H
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <std_msgs/Float64.h>
|
||||||
|
#include <sensor_msgs/NavSatFix.h>
|
||||||
|
#include <class_model/gps_location.h>
|
||||||
|
|
||||||
|
class ThreadGPSClass {
|
||||||
|
public:
|
||||||
|
ThreadGPSClass();
|
||||||
|
virtual ~ThreadGPSClass();
|
||||||
|
global_location gps_position();
|
||||||
|
float get_heading();
|
||||||
|
|
||||||
|
private:
|
||||||
|
// ROS NodeHandle
|
||||||
|
ros::NodeHandle node_handle_;
|
||||||
|
|
||||||
|
// SUBSCRIBE
|
||||||
|
ros::Subscriber gps_sub;
|
||||||
|
ros::Subscriber compass_sub;
|
||||||
|
|
||||||
|
void GPS_callback(const sensor_msgs::NavSatFix::ConstPtr &gps);
|
||||||
|
void Compass_callback(const std_msgs::Float64::ConstPtr °ree);
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // SENSOR_H
|
||||||
@ -0,0 +1,27 @@
|
|||||||
|
<launch>
|
||||||
|
<group>
|
||||||
|
<node pkg="class_model" type="main" name="leader" output="screen" ns="/drone1">
|
||||||
|
<param name="namespace" value="/drone1"/>
|
||||||
|
<param name="droneID" value="1"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
|
||||||
|
<group>
|
||||||
|
|
||||||
|
<node pkg="class_model" type="main" name="follower1" output="screen" ns="/drone2">
|
||||||
|
<param name="namespace" value="/drone2"/>
|
||||||
|
<param name="droneID" value="2"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
|
||||||
|
<group>
|
||||||
|
|
||||||
|
<node pkg="class_model" type="main" name="follower2" output="screen" ns="/drone3">
|
||||||
|
<param name="namespace" value="/drone3"/>
|
||||||
|
<param name="droneID" value="3"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
</launch>
|
||||||
@ -0,0 +1,26 @@
|
|||||||
|
<launch>
|
||||||
|
<!-- vim: set ft=xml noet : -->
|
||||||
|
<!-- Experimental launch script for APM based Drone -->
|
||||||
|
|
||||||
|
<arg name="fcu_url" default="udp://127.0.0.1:14551@14555" />
|
||||||
|
<arg name="gcs_url" default="" />
|
||||||
|
<arg name="tgt_system" default="1" />
|
||||||
|
<arg name="tgt_component" default="1" />
|
||||||
|
<arg name="log_output" default="screen" />
|
||||||
|
<arg name="respawn_mavros" default="true"/>
|
||||||
|
<arg name="mavros_ns" default="/"/>
|
||||||
|
<arg name="drone_ID" default="1" />
|
||||||
|
<arg name="config_yaml" default="$(find mavros)/launch/apm_config.yaml" />
|
||||||
|
|
||||||
|
<include file="$(find iq_sim)/launch/mavros_node.launch">
|
||||||
|
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
|
||||||
|
<arg name="config_yaml" value="$(arg config_yaml)" />
|
||||||
|
<arg name="mavros_ns" value="$(arg mavros_ns)"/>
|
||||||
|
<arg name="fcu_url" value="$(arg fcu_url)" />
|
||||||
|
<arg name="gcs_url" value="$(arg gcs_url)" />
|
||||||
|
<arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
|
||||||
|
<arg name="tgt_system" value="$(arg tgt_system)" />
|
||||||
|
<arg name="tgt_component" value="$(arg tgt_component)" />
|
||||||
|
<arg name="log_output" value="$(arg log_output)" />
|
||||||
|
</include>
|
||||||
|
</launch>
|
||||||
@ -0,0 +1,45 @@
|
|||||||
|
<launch>
|
||||||
|
<group>
|
||||||
|
<node pkg="class_model" type="main" name="leader" output="screen" ns="/drone1">
|
||||||
|
<param name="namespace" value="/drone1"/>
|
||||||
|
<param name="droneID" value="1"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
|
||||||
|
<group>
|
||||||
|
|
||||||
|
<node pkg="class_model" type="main" name="follower1" output="screen" ns="/drone2">
|
||||||
|
<param name="namespace" value="/drone2"/>
|
||||||
|
<param name="droneID" value="2"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
|
||||||
|
<group>
|
||||||
|
|
||||||
|
<node pkg="class_model" type="main" name="follower2" output="screen" ns="/drone3">
|
||||||
|
<param name="namespace" value="/drone3"/>
|
||||||
|
<param name="droneID" value="3"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
|
||||||
|
<group>
|
||||||
|
|
||||||
|
<node pkg="class_model" type="main" name="follower3" output="screen" ns="/drone4">
|
||||||
|
<param name="namespace" value="/drone4"/>
|
||||||
|
<param name="droneID" value="4"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
|
||||||
|
<group>
|
||||||
|
|
||||||
|
<node pkg="class_model" type="main" name="follower4" output="screen" ns="/drone5">
|
||||||
|
<param name="namespace" value="/drone5"/>
|
||||||
|
<param name="droneID" value="5"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
</launch>
|
||||||
@ -0,0 +1,54 @@
|
|||||||
|
<launch>
|
||||||
|
<group>
|
||||||
|
<node pkg="class_model" type="main" name="leader" output="screen" ns="/drone1">
|
||||||
|
<param name="namespace" value="/drone1"/>
|
||||||
|
<param name="droneID" value="1"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
|
||||||
|
<group>
|
||||||
|
|
||||||
|
<node pkg="class_model" type="main" name="follower1" output="screen" ns="/drone2">
|
||||||
|
<param name="namespace" value="/drone2"/>
|
||||||
|
<param name="droneID" value="2"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
|
||||||
|
<group>
|
||||||
|
|
||||||
|
<node pkg="class_model" type="main" name="follower2" output="screen" ns="/drone3">
|
||||||
|
<param name="namespace" value="/drone3"/>
|
||||||
|
<param name="droneID" value="3"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
|
||||||
|
<group>
|
||||||
|
|
||||||
|
<node pkg="class_model" type="main" name="follower3" output="screen" ns="/drone4">
|
||||||
|
<param name="namespace" value="/drone4"/>
|
||||||
|
<param name="droneID" value="4"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
|
||||||
|
<group>
|
||||||
|
|
||||||
|
<node pkg="class_model" type="main" name="follower4" output="screen" ns="/drone5">
|
||||||
|
<param name="namespace" value="/drone5"/>
|
||||||
|
<param name="droneID" value="5"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
|
||||||
|
<group>
|
||||||
|
|
||||||
|
<node pkg="class_model" type="main" name="follower5" output="screen" ns="/drone6">
|
||||||
|
<param name="namespace" value="/drone6"/>
|
||||||
|
<param name="droneID" value="6"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
</launch>
|
||||||
@ -0,0 +1,19 @@
|
|||||||
|
<launch>
|
||||||
|
<group>
|
||||||
|
<node pkg="class_model" type="main" name="leader" output="screen" ns="/drone1">
|
||||||
|
<param name="namespace" value="/drone1"/>
|
||||||
|
<param name="droneID" value="1"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
|
||||||
|
<group>
|
||||||
|
|
||||||
|
<node pkg="class_model" type="main" name="follower1" output="screen" ns="/drone2">
|
||||||
|
<param name="namespace" value="/drone2"/>
|
||||||
|
<param name="droneID" value="2"/>
|
||||||
|
<param name="use_sim_time" value="true" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
|
||||||
|
</launch>
|
||||||
@ -0,0 +1,68 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<package format="2">
|
||||||
|
<name>class_model</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>The class_model package</description>
|
||||||
|
|
||||||
|
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||||
|
<!-- Example: -->
|
||||||
|
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||||
|
<maintainer email="dodo@todo.todo">dodo</maintainer>
|
||||||
|
|
||||||
|
|
||||||
|
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||||
|
<!-- Commonly used license strings: -->
|
||||||
|
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||||
|
<license>TODO</license>
|
||||||
|
|
||||||
|
|
||||||
|
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||||
|
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||||
|
<!-- Example: -->
|
||||||
|
<!-- <url type="website">http://wiki.ros.org/class_model</url> -->
|
||||||
|
|
||||||
|
|
||||||
|
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||||
|
<!-- Authors do not have to be maintainers, but could be -->
|
||||||
|
<!-- Example: -->
|
||||||
|
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||||
|
|
||||||
|
|
||||||
|
<!-- The *depend tags are used to specify dependencies -->
|
||||||
|
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||||
|
<!-- Examples: -->
|
||||||
|
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||||
|
<!-- <depend>roscpp</depend> -->
|
||||||
|
<!-- Note that this is equivalent to the following: -->
|
||||||
|
<!-- <build_depend>roscpp</build_depend> -->
|
||||||
|
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||||
|
<!-- Use build_depend for packages you need at compile time: -->
|
||||||
|
<!-- <build_depend>message_generation</build_depend> -->
|
||||||
|
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||||
|
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||||
|
<!-- Use buildtool_depend for build tool packages: -->
|
||||||
|
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||||
|
<!-- Use exec_depend for packages you need at runtime: -->
|
||||||
|
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||||
|
<!-- Use test_depend for packages you need only for testing: -->
|
||||||
|
<!-- <test_depend>gtest</test_depend> -->
|
||||||
|
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||||
|
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||||
|
<buildtool_depend>catkin</buildtool_depend>
|
||||||
|
<build_depend>roscpp</build_depend>
|
||||||
|
<build_depend>rospy</build_depend>
|
||||||
|
<build_depend>std_msgs</build_depend>
|
||||||
|
<build_export_depend>roscpp</build_export_depend>
|
||||||
|
<build_export_depend>rospy</build_export_depend>
|
||||||
|
<build_export_depend>std_msgs</build_export_depend>
|
||||||
|
<exec_depend>roscpp</exec_depend>
|
||||||
|
<exec_depend>rospy</exec_depend>
|
||||||
|
<exec_depend>std_msgs</exec_depend>
|
||||||
|
|
||||||
|
|
||||||
|
<!-- The export tag contains other, unspecified, tags -->
|
||||||
|
<export>
|
||||||
|
<!-- Other tools can request additional information be placed here -->
|
||||||
|
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
@ -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;
|
||||||
|
}
|
||||||
@ -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()
|
||||||
@ -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<geometry_msgs::TwistStamped>(ros_namespace+"/mavros/setpoint_velocity/cmd_vel",100);
|
||||||
|
gps_command=node_handle_.advertise<mavros_msgs::GlobalPositionTarget>(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::milliseconds>(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::milliseconds>(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::milliseconds>(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::milliseconds>(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::milliseconds>(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::milliseconds>(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;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@ -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()
|
||||||
@ -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<mavros_msgs::CommandBool>(ros_namespace+"/mavros/cmd/arming");
|
||||||
|
takeoff_client=node_handle_.serviceClient<mavros_msgs::CommandTOL>(ros_namespace+"/mavros/cmd/takeoff");
|
||||||
|
state_sub = node_handle_.subscribe<mavros_msgs::State>(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;
|
||||||
|
|
||||||
|
}
|
||||||
@ -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;
|
||||||
|
}
|
||||||
@ -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);
|
||||||
|
|
||||||
|
}
|
||||||
@ -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<std_msgs::String>(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 <<std::endl;
|
||||||
|
}
|
||||||
|
if(counter>=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);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
@ -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()
|
||||||
@ -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;
|
||||||
|
}
|
||||||
@ -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);
|
||||||
|
}
|
||||||
@ -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<mavros_msgs::SetMode>(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;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@ -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;
|
||||||
|
}
|
||||||
@ -0,0 +1,75 @@
|
|||||||
|
#include"class_model/requestData.h"
|
||||||
|
#include <cstdlib>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
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;i<data.length();i++){
|
||||||
|
if(data[i] == ':'){
|
||||||
|
i+=3;
|
||||||
|
while(data[i] != ','){
|
||||||
|
list[j]=list[j]+data[i];
|
||||||
|
i++;
|
||||||
|
if(data[i] == ',' || data[i] == '}'){
|
||||||
|
j++;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if(j>3){
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
leader_position.lat=std::stoi(list[0]);
|
||||||
|
leader_position.lon=std::stoi(list[1]);
|
||||||
|
leader_position.alt=std::stoi(list[2]);
|
||||||
|
heading = std::stoi(list[3]);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// std::cout<<sizeof(data)<<std::endl;
|
||||||
|
//std::cout<<data<<std::endl;
|
||||||
|
// ROS_INFO("leader_GPS [%f,%f]", leader_position.lat, leader_position.lon);
|
||||||
|
// ROS_INFO("leader_heading: %f",heading);
|
||||||
|
}
|
||||||
@ -0,0 +1,178 @@
|
|||||||
|
#include "class_model/select.h"
|
||||||
|
|
||||||
|
|
||||||
|
SelectClass::SelectClass() : node_handle_(""){
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
SelectClass::~SelectClass() { ros::shutdown(); }
|
||||||
|
|
||||||
|
void SelectClass::init_formation(float x ,float y){
|
||||||
|
|
||||||
|
counter = 0;
|
||||||
|
// if(param_object.getID() == 1){
|
||||||
|
// formation_object.leader();
|
||||||
|
// }else if(param_object.getID() == 2){
|
||||||
|
// formation_object.follower1(counter);
|
||||||
|
// }else if(param_object.getID() == 3){
|
||||||
|
// formation_object.follower2(counter);
|
||||||
|
// }else if(param_object.getID() == 4){
|
||||||
|
// formation_object.follower3(counter);
|
||||||
|
// }else if(param_object.getID() == 5){
|
||||||
|
// formation_object.follower4(counter);
|
||||||
|
// }
|
||||||
|
|
||||||
|
if(param_object.getID() == 1){
|
||||||
|
formation_object.leader1(x,y);
|
||||||
|
}else if(param_object.getID() == 2){
|
||||||
|
formation_object.sph_follower1(counter);
|
||||||
|
}else if(param_object.getID() == 3){
|
||||||
|
formation_object.sph_follower2(counter);
|
||||||
|
}else if(param_object.getID() == 4){
|
||||||
|
formation_object.sph_follower3(counter);
|
||||||
|
}else if(param_object.getID() == 5){
|
||||||
|
formation_object.sph_follower4(counter);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// sleep(2);
|
||||||
|
|
||||||
|
// mode_object.set_Mode("LAND");
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void SelectClass::goose_formation(float x ,float y){
|
||||||
|
|
||||||
|
counter = 0;
|
||||||
|
if(param_object.getID() == 1){
|
||||||
|
formation_object.leader1();
|
||||||
|
}else if(param_object.getID() == 2){
|
||||||
|
formation_object.follower1(counter);
|
||||||
|
}else if(param_object.getID() == 3){
|
||||||
|
formation_object.follower2(counter);
|
||||||
|
}else if(param_object.getID() == 4){
|
||||||
|
formation_object.follower3(counter);
|
||||||
|
}else if(param_object.getID() == 5){
|
||||||
|
formation_object.follower4(counter);
|
||||||
|
}else if(param_object.getID() == 6){
|
||||||
|
formation_object.follower5(counter);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// sleep(2);
|
||||||
|
|
||||||
|
// mode_object.set_Mode("LAND");
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void SelectClass::line_formation(float x ,float y){
|
||||||
|
|
||||||
|
counter = 1;
|
||||||
|
if(param_object.getID() == 1){
|
||||||
|
formation_object.leader1();
|
||||||
|
}else if(param_object.getID() == 2){
|
||||||
|
formation_object.follower1(counter);
|
||||||
|
}else if(param_object.getID() == 3){
|
||||||
|
formation_object.follower2(counter);
|
||||||
|
}else if(param_object.getID() == 4){
|
||||||
|
formation_object.follower3(counter);
|
||||||
|
}else if(param_object.getID() == 5){
|
||||||
|
formation_object.follower4(counter);
|
||||||
|
}else if(param_object.getID() == 6){
|
||||||
|
formation_object.follower5(counter);
|
||||||
|
}
|
||||||
|
// sleep(2);
|
||||||
|
|
||||||
|
// mode_object.set_Mode("LAND");
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void SelectClass::row_formation(){
|
||||||
|
|
||||||
|
counter = 2;
|
||||||
|
if(param_object.getID() == 1){
|
||||||
|
formation_object.leader1();
|
||||||
|
}else if(param_object.getID() == 2){
|
||||||
|
formation_object.follower1(counter);
|
||||||
|
}else if(param_object.getID() == 3){
|
||||||
|
formation_object.follower2(counter);
|
||||||
|
}else if(param_object.getID() == 4){
|
||||||
|
formation_object.follower3(counter);
|
||||||
|
}else if(param_object.getID() == 5){
|
||||||
|
formation_object.follower4(counter);
|
||||||
|
}else if(param_object.getID() == 6){
|
||||||
|
formation_object.follower5(counter);
|
||||||
|
}
|
||||||
|
// sleep(2);
|
||||||
|
|
||||||
|
// mode_object.set_Mode("LAND");
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void SelectClass::circle_formation(){
|
||||||
|
|
||||||
|
counter = 3;
|
||||||
|
if(param_object.getID() == 1){
|
||||||
|
formation_object.leader1();
|
||||||
|
}else if(param_object.getID() == 2){
|
||||||
|
formation_object.follower1(counter);
|
||||||
|
}else if(param_object.getID() == 3){
|
||||||
|
formation_object.follower2(counter);
|
||||||
|
}else if(param_object.getID() == 4){
|
||||||
|
formation_object.follower3(counter);
|
||||||
|
}else if(param_object.getID() == 5){
|
||||||
|
formation_object.follower4(counter);
|
||||||
|
}else if(param_object.getID() == 6){
|
||||||
|
formation_object.follower5(counter);
|
||||||
|
}
|
||||||
|
// sleep(2);
|
||||||
|
|
||||||
|
// mode_object.set_Mode("LAND");
|
||||||
|
}
|
||||||
|
|
||||||
|
void SelectClass::protect_formation(float x ,float y){
|
||||||
|
|
||||||
|
counter = 4;
|
||||||
|
if(param_object.getID() == 1){
|
||||||
|
formation_object.leader1(x,y);
|
||||||
|
}else if(param_object.getID() == 2){
|
||||||
|
formation_object.follower1(counter);
|
||||||
|
}else if(param_object.getID() == 3){
|
||||||
|
formation_object.follower2(counter);
|
||||||
|
}else if(param_object.getID() == 4){
|
||||||
|
formation_object.follower3(counter);
|
||||||
|
}else if(param_object.getID() == 5){
|
||||||
|
formation_object.follower4(counter);
|
||||||
|
}else if(param_object.getID() == 6){
|
||||||
|
formation_object.follower5(counter);
|
||||||
|
}
|
||||||
|
// sleep(2);
|
||||||
|
|
||||||
|
// mode_object.set_Mode("LAND");
|
||||||
|
}
|
||||||
|
|
||||||
|
void SelectClass::Hex_formation(float x ,float y){
|
||||||
|
|
||||||
|
counter = 5;
|
||||||
|
if(param_object.getID() == 1){
|
||||||
|
formation_object.leader1(x,y);
|
||||||
|
}else if(param_object.getID() == 2){
|
||||||
|
formation_object.follower1(counter);
|
||||||
|
}else if(param_object.getID() == 3){
|
||||||
|
formation_object.follower2(counter);
|
||||||
|
}else if(param_object.getID() == 4){
|
||||||
|
formation_object.follower3(counter);
|
||||||
|
}else if(param_object.getID() == 5){
|
||||||
|
formation_object.follower4(counter);
|
||||||
|
}else if(param_object.getID() == 6){
|
||||||
|
formation_object.follower5(counter);
|
||||||
|
}
|
||||||
|
// sleep(2);
|
||||||
|
|
||||||
|
// mode_object.set_Mode("LAND");
|
||||||
|
}
|
||||||
|
|
||||||
|
void SelectClass::stop(){
|
||||||
|
|
||||||
|
}
|
||||||
@ -0,0 +1,45 @@
|
|||||||
|
#include <class_model/sensor.h>
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
@ -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)
|
||||||
Loading…
Reference in New Issue