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