Texas Aerial Robotics Documentation
control_functions.hpp
Go to the documentation of this file.
1 #include <mavros_msgs/CommandTOL.h>
2 #include <mavros_msgs/State.h>
3 #include <nav_msgs/Odometry.h>
4 #include <geometry_msgs/Pose.h>
5 #include <geometry_msgs/PoseStamped.h>
6 #include <cmath>
7 #include <math.h>
8 #include <ros/ros.h>
9 #include <std_msgs/Float64.h>
10 #include <std_msgs/String.h>
11 #include <mavros_msgs/CommandBool.h>
12 #include <mavros_msgs/SetMode.h>
13 #include <mavros_msgs/PositionTarget.h>
14 #include <unistd.h>
15 #include <vector>
16 #include <ros/duration.h>
17 #include <iostream>
18 
19 
26 mavros_msgs::State current_state_g;
27 nav_msgs::Odometry current_pose_g;
28 geometry_msgs::Pose correction_vector_g;
29 geometry_msgs::PoseStamped waypoint_g;
30 
34 
35 
36 
37 ros::Publisher local_pos_pub;
38 ros::Subscriber currentPos;
39 ros::Subscriber state_sub;
40 ros::ServiceClient arming_client;
41 ros::ServiceClient land_client;
42 ros::ServiceClient set_mode_client;
43 ros::ServiceClient takeoff_client;
44 
50  float x;
51  float y;
52  float z;
53  float psi;
54 };
55 
56 //get armed state
57 void state_cb(const mavros_msgs::State::ConstPtr& msg)
58 {
59  current_state_g = *msg;
60 }
61 void enu_2_local(nav_msgs::Odometry current_pose_enu)
62 {
63  float x = current_pose_enu.pose.pose.position.x;
64  float y = current_pose_enu.pose.pose.position.y;
65  float z = current_pose_enu.pose.pose.position.z;
66  float deg2rad = (M_PI/180);
67  float X = x*cos(local_offset_g*deg2rad) - y*sin(local_offset_g*deg2rad);
68  float Y = x*sin(local_offset_g*deg2rad) + y*cos(local_offset_g*deg2rad);
69  float Z = z;
70  //ROS_INFO("Local position %f %f %f",X, Y, Z);
71 }
72 //get current position of drone
73 void pose_cb(const nav_msgs::Odometry::ConstPtr& msg)
74 {
75  current_pose_g = *msg;
77  float q0 = current_pose_g.pose.pose.orientation.w;
78  float q1 = current_pose_g.pose.pose.orientation.x;
79  float q2 = current_pose_g.pose.pose.orientation.y;
80  float q3 = current_pose_g.pose.pose.orientation.z;
81  float psi = atan2((2*(q0*q3 + q1*q2)), (1 - 2*(pow(q2,2) + pow(q3,2))) );
82  //ROS_INFO("Current Heading %f ENU", psi*(180/M_PI));
83  //Heading is in ENU
84  current_heading_g = psi*(180/M_PI) - local_offset_g;
85  //ROS_INFO("Current Heading %f origin", current_heading_g);
86  //ROS_INFO("x: %f y: %f z: %f", current_pose_g.pose.pose.position.x, current_pose_g.pose.pose.position.y, current_pose_g.pose.pose.position.z);
87 }
88 //set orientation of the drone (drone should always be level)
89 // Heading input should match the ENU coordinate system
95 void set_heading(float heading)
96 {
97  heading = heading + correction_heading_g + local_offset_g;
98  float yaw = heading*(M_PI/180);
99  float pitch = 0;
100  float roll = 0;
101 
102  float cy = cos(yaw * 0.5);
103  float sy = sin(yaw * 0.5);
104  float cr = cos(roll * 0.5);
105  float sr = sin(roll * 0.5);
106  float cp = cos(pitch * 0.5);
107  float sp = sin(pitch * 0.5);
108 
109  float qw = cy * cr * cp + sy * sr * sp;
110  float qx = cy * sr * cp - sy * cr * sp;
111  float qy = cy * cr * sp + sy * sr * cp;
112  float qz = sy * cr * cp - cy * sr * sp;
113 
114  waypoint_g.pose.orientation.w = qw;
115  waypoint_g.pose.orientation.x = qx;
116  waypoint_g.pose.orientation.y = qy;
117  waypoint_g.pose.orientation.z = qz;
118 }
119 // set position to fly to in the local frame
125 void set_destination(float x, float y, float z, float psi)
126 {
127  set_heading(psi);
128  //transform map to local
129  float deg2rad = (M_PI/180);
130  float Xlocal = x*cos((correction_heading_g + local_offset_g - 90)*deg2rad) - y*sin((correction_heading_g + local_offset_g - 90)*deg2rad);
131  float Ylocal = x*sin((correction_heading_g + local_offset_g - 90)*deg2rad) + y*cos((correction_heading_g + local_offset_g - 90)*deg2rad);
132  float Zlocal = z;
133 
134  x = Xlocal + correction_vector_g.position.x;
135  y = Ylocal + correction_vector_g.position.y;
136  z = Zlocal + correction_vector_g.position.z;
137  ROS_INFO("Destination set to x: %f y: %f z: %f origin frame", x, y, z);
138 
139  waypoint_g.pose.position.x = x;
140  waypoint_g.pose.position.y = y;
141  waypoint_g.pose.position.z = z;
142 
143 }
151 {
152  ROS_INFO("Waiting for FCU connection");
153  // wait for FCU connection
154  while (ros::ok() && !current_state_g.connected)
155  {
156  ros::spinOnce();
157  ros::Duration(0.01).sleep();
158  }
159  if(current_state_g.connected)
160  {
161  ROS_INFO("Connected to FCU");
162  return 0;
163  }else{
164  ROS_INFO("Error connecting to drone");
165  return -1;
166  }
167 
168 
169 }
177 {
178  ROS_INFO("Waiting for user to set mode to GUIDED");
179  while(ros::ok() && current_state_g.mode != "GUIDED")
180  {
181  ros::spinOnce();
182  ros::Duration(0.01).sleep();
183  }
184  if(current_state_g.mode == "GUIDED")
185  {
186  ROS_INFO("Mode set to GUIDED. Mission starting");
187  return 0;
188  }else{
189  ROS_INFO("Error starting mission!!");
190  return -1;
191  }
192 }
199 {
200  //set the orientation of the local reference frame
201  ROS_INFO("Initializing local coordinate system");
202  local_offset_g = 0;
203  for (int i = 1; i <= 30; i++) {
204  ros::spinOnce();
205  ros::Duration(0.1).sleep();
206 
207  float q0 = current_pose_g.pose.pose.orientation.w;
208  float q1 = current_pose_g.pose.pose.orientation.x;
209  float q2 = current_pose_g.pose.pose.orientation.y;
210  float q3 = current_pose_g.pose.pose.orientation.z;
211  float psi = atan2((2*(q0*q3 + q1*q2)), (1 - 2*(pow(q2,2) + pow(q3,2))) ); // yaw
212 
213  local_offset_g += psi*(180/M_PI);
214  // ROS_INFO("current heading%d: %f", i, local_offset_g/i);
215  }
216  local_offset_g /= 30;
217  ROS_INFO("Coordinate offset set");
218  ROS_INFO("the X' axis is facing: %f", local_offset_g);
219  return 0;
220 }
228 int takeoff(float takeoff_alt)
229 {
230  //intitialize first waypoint of mission
231  set_destination(0,0,takeoff_alt,0);
232  for(int i=0; i<100; i++)
233  {
234  local_pos_pub.publish(waypoint_g);
235  ros::spinOnce();
236  ros::Duration(0.01).sleep();
237  }
238  // arming
239  ROS_INFO("Arming drone");
240  mavros_msgs::CommandBool arm_request;
241  arm_request.request.value = true;
242  while (!current_state_g.armed && !arm_request.response.success)
243  {
244  ros::Duration(.1).sleep();
245  arming_client.call(arm_request);
246  local_pos_pub.publish(waypoint_g);
247  }
248  if(arm_request.response.success)
249  {
250  ROS_INFO("Arming Successful");
251  }else{
252  ROS_INFO("Arming failed with %d", arm_request.response.success);
253  return -1;
254  }
255 
256  //request takeoff
257 
258  mavros_msgs::CommandTOL srv_takeoff;
259  srv_takeoff.request.altitude = takeoff_alt;
260  if(takeoff_client.call(srv_takeoff)){
261  ROS_INFO("takeoff sent %d", srv_takeoff.response.success);
262  }else{
263  ROS_ERROR("Failed Takeoff");
264  return -2;
265  }
266  sleep(5);
267  return 0;
268 }
276 {
277  local_pos_pub.publish(waypoint_g);
278 
279  float tollorance = .3;
280  float deltaX = abs(waypoint_g.pose.position.x - current_pose_g.pose.pose.position.x);
281  float deltaY = abs(waypoint_g.pose.position.y - current_pose_g.pose.pose.position.y);
282  float deltaZ = abs(waypoint_g.pose.position.z - current_pose_g.pose.pose.position.z);
283  float dMag = sqrt( pow(deltaX, 2) + pow(deltaY, 2) + pow(deltaZ, 2) );
284 
285  if( dMag < tollorance)
286  {
287  return 1;
288  }else{
289  return 0;
290  }
291 }
298 int land()
299 {
300  mavros_msgs::CommandTOL srv_land;
301  if(land_client.call(srv_land) && srv_land.response.success)
302  {
303  ROS_INFO("land sent %d", srv_land.response.success);
304  return 0;
305  }else{
306  ROS_ERROR("Landing failed");
307  return -1;
308  }
309 }
315 int init_publisher_subscriber(ros::NodeHandle controlnode)
316 {
317  local_pos_pub = controlnode.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
318  currentPos = controlnode.subscribe<nav_msgs::Odometry>("mavros/global_position/local", 10, pose_cb);
319  state_sub = controlnode.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
320  arming_client = controlnode.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
321  land_client = controlnode.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/land");
322  set_mode_client = controlnode.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
323  takeoff_client = controlnode.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/takeoff");
324 
325 }
326 
float correction_heading_g
ros::Subscriber state_sub
geometry_msgs::Pose correction_vector_g
void set_heading(float heading)
float y
distance in y with respect to your reference frame
float psi
rotation about the third axis of your reference frame
nav_msgs::Odometry current_pose_g
ros::ServiceClient land_client
void enu_2_local(nav_msgs::Odometry current_pose_enu)
void set_destination(float x, float y, float z, float psi)
ros::ServiceClient takeoff_client
int check_waypoint_reached()
int init_publisher_subscriber(ros::NodeHandle controlnode)
ros::ServiceClient set_mode_client
geometry_msgs::PoseStamped waypoint_g
ros::Subscriber currentPos
mavros_msgs::State current_state_g
float current_heading_g
int takeoff(float takeoff_alt)
void pose_cb(const nav_msgs::Odometry::ConstPtr &msg)
float z
distance in z with respect to your reference frame
int land()
float local_offset_g
ros::Publisher local_pos_pub
int wait4connect()
int initialize_local_frame()
ros::ServiceClient arming_client
int wait4start()
float x
distance in x with respect to your reference frame
void state_cb(const mavros_msgs::State::ConstPtr &msg)