You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
74 lines
2.1 KiB
C++
74 lines
2.1 KiB
C++
/*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/receiver.h"
|
|
#include "class_model/requestData.h"
|
|
#include "class_model/DroneStatus.h"
|
|
#include "class_model/PID.h"
|
|
#include "class_model/convert_degree.h"
|
|
#include "yaml-cpp/yaml.h"
|
|
|
|
|
|
class FollowerClass {
|
|
public:
|
|
FollowerClass();
|
|
virtual ~FollowerClass();
|
|
//ClassObject
|
|
ThreadGPSClass GPS_object;
|
|
ModeClass mode_object;
|
|
ControlClass control_object;
|
|
ReceiverClass receiver_object;
|
|
CommandClass command_object;
|
|
RequestClass request_object;
|
|
PIDClass PID_x;
|
|
PIDClass PID_y;
|
|
DroneStatus drone1;
|
|
DroneStatus drone2;
|
|
DroneStatus drone3;
|
|
DroneStatus drone4;
|
|
DroneStatus drone5;
|
|
DroneStatus self;
|
|
DroneStatus samePlane[5];
|
|
|
|
void follower(global_location** member_data, size_t index, int refID);
|
|
int find_ref_drone(global_location** data, size_t index, int leaderID);
|
|
|
|
float lon[3],lat[3];
|
|
global_location vector_LT,vector_SM;
|
|
|
|
|
|
private:
|
|
// ROS NodeHandle
|
|
ros::NodeHandle node_handle_;
|
|
|
|
global_location target_location;
|
|
global_location follower_location;
|
|
global_location leader_location;
|
|
global_location leader_drone;
|
|
global_location refpos;
|
|
DroneStatus memberDrone[5];
|
|
global_location (RequestClass::*ref)();
|
|
|
|
YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");
|
|
float distance = config["formation"]["distance"].as<float>();
|
|
float angle = config["formation"]["angle"].as<float>();
|
|
float error_lon,error_lat;
|
|
float follower_pid[3] = {1 , 0.00000000001 ,0};// D downsize
|
|
float ignore_small = 0.50;
|
|
int flag = 0;
|
|
|
|
void calculate_position(float k,float theta,int plane);
|
|
void plane(global_location* target, global_location* leader, global_location* follower);
|
|
int direct(global_location* target, global_location* leader, DroneStatus self, DroneStatus member);
|
|
void choose_leader();
|
|
|
|
|
|
};
|
|
|
|
#endif // FOLLOWER_H
|