forked from IeiuniumLux/Visual-SLAM
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathModeHeader.h
executable file
·64 lines (57 loc) · 1.36 KB
/
ModeHeader.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
#ifndef _MODEHEADER_H
#define _MODEHEADER_H
#include <ros/ros.h>
#include <ros/package.h>
#include <iostream>
#include <fstream>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include "geometry_msgs/PoseStamped.h"
#include <tf/transform_broadcaster.h>
using namespace std;
//Node handle pointer
extern ros::NodeHandle * nh;
//Extern variables needed by the modes
extern ros::Time last_request;
extern string desired_mode;
extern geometry_msgs::PoseStamped DesiredPoseStamped;
extern float current_x, current_y, current_z, start_x, start_y;
extern float takeoff_height;
extern double current_yaw;
extern mavros_msgs::State current_state;
extern bool armNow, disarmNow, flymode, publishFromGCS, followWaypoint;
extern vector<float> xWaypoint, yWaypoint, zWaypoint, yawWaypoint, tWaypoint;
extern int nWaypoints, desWaypoint;
/* --- Navigation modes --- */
//Offboard
void mode_O();
//Arm
void mode_A();
//Takeoff
void mode_T();
//Land
void mode_L();
//Disarm
void mode_DISARM();
//Home position
void mode_H();
//Record waypoint
void mode_R();
//Waypoint position
void mode_W();
//Next waypoint position
void mode_N();
//Follow waypoints
void mode_F();
//Search for objects
void mode_SEARCH();
//Clear waypoint
void mode_CW();
//Load waypoints
void mode_LW();
//Save waypoints
void mode_SW();
//Start flying
void mode_FLY();
#endif