Upload files to 'src'

src/LinkState.py

@@ -0,0 +1,242 @@
+#!/usr/bin/env python2.7
+
+### Cesar Rodriguez June 2021
+### Script to determine payload and drone state from Gazebo
+
+import rospy,  tf
+import rosservice
+import time
+import math
+from tf.transformations import *
+from offboard_ex.msg import tethered_status
+from geometry_msgs.msg import Pose
+from gazebo_msgs.srv import GetLinkState
+from std_msgs.msg import Bool
+
+class Main:
+
+	def __init__(self):
+
+		# rate(s)
+		rate = 40 # rate for the publisher method, specified in Hz -- 20 Hz
+
+		self.dt		 = 1.0/rate
+
+		# Variables needed for testing start 
+		self.tstart		= rospy.get_time() # Keep track of the start time
+		while self.tstart == 0.0:				 # Need to make sure get_rostime works
+			self.tstart		= rospy.get_time()
+		# How long should we wait before before starting test
+		self.param_exists = False
+		while self.param_exists == False:
+			if rospy.has_param('sim/wait'):
+				self.wait	= rospy.get_param('sim/wait') # wait time
+				self.param_exists = True
+			elif rospy.get_time() - self.tstart >= 3.0:
+				break
+	
+		# Will be set to true when test should start
+		self.bool = False
+
+		# initialize variables
+		self.phi			= 0.0 #	Payload angle of deflection from x-axis
+		self.theta		= 0.0 # Payload angle of deflection from y-axis
+		self.tetherL	= 0.0 # Tether length
+		self.has_run	= 0		# Boolean to keep track of first run instance
+		self.phidot		= 0.0 #	
+		self.thetadot	= 0.0 #
+		self.phibuf	  =	0.0 # Need buffers to determine their rates
+		self.thetabuf = 0.0 #
+		self.pload		= True # Check if payload exists
+		# Max dot values to prevent 'blowup'
+		self.phidot_max   = 3.0
+		self.thetadot_max = 3.0 
+
+		# variables for message gen
+		self.status 					= tethered_status()
+		self.status.drone_id	= 'spiri_with_tether::spiri::base'
+		self.status.drone_pos	= Pose()
+		self.status.pload_id	= 'spiri_with_tether::mass::payload'
+		self.status.pload_pos	= Pose()
+		self.status.phi				= 0.0
+		self.status.theta 		= 0.0
+
+		# service(s)
+		self.service1 = '/gazebo/get_link_state'
+
+		# need service list to check if models have spawned
+		self.service_list = rosservice.get_service_list()
+
+		# wait for service to exist
+		while self.service1 not in self.service_list:
+			print "Waiting for models to spawn..."
+			self.service_list = rosservice.get_service_list()
+			if rospy.get_time() - self.tstart >= 10.0:
+				break
+
+		# publisher(s)
+		self.publisher = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1)
+		self.pub_wd	 	 = rospy.Publisher('/status/path_follow', Bool, queue_size=1)
+
+		self.pub_timer  = rospy.Timer(rospy.Duration(1.0/rate), self.link_state)
+		self.path_timer = rospy.Timer(rospy.Duration(40.0/rate), self.path_follow)
+	
+	def cutoff(self,value,ceiling):
+		"""
+		Takes in value and returns ceiling
+		if value > ceiling. Otherwise, it returns
+		value back
+		"""
+	  # initilize sign
+		sign   = 1
+	
+		# check if value is negative
+		if value < 0.0:
+			sign = -1
+		# Cutoff value at ceiling
+		if (value > ceiling or value < -ceiling):
+			output = sign*ceiling
+		else:
+			output = value
+		return output
+
+	def euler_array(self,pose):
+		"""
+		Takes in pose msg object and outputs array of euler angs:
+			q[0] = Roll
+			q[1] = Pitch
+			q[2] = Yaw
+		"""
+		q = euler_from_quaternion([pose.orientation.x,
+															pose.orientation.y,
+															pose.orientation.z,
+															pose.orientation.w])
+		return q
+		
+	# Get link states (drone and pload) and determine angle between them
+	def link_state(self,pub_timer):
+		
+		try:
+
+			# State which service we are querying
+			get_P = rospy.ServiceProxy(self.service1,GetLinkState)
+
+			# Set reference frame
+			reference	= ''
+
+			# Establish links needed --> Spiri base and payload
+			# P = Position vector
+			drone_P = get_P(self.status.drone_id,reference)
+
+			# Get orientation of drone in euler angles
+			drone_Eul = self.euler_array(drone_P.link_state.pose)
+
+			# Check if payload is part of simulation
+			if not drone_P.success:
+				self.status.drone_id	= 'spiri::base'
+				drone_P = get_P(self.status.drone_id,reference) # i.e. no payload
+				self.pload = False
+
+			pload_P = get_P(self.status.pload_id,reference)
+
+			if not self.has_run == 1:
+				if self.pload == True:
+					# Get tether length based off initial displacement
+					self.tetherL = math.sqrt((drone_P.link_state.pose.position.x - 
+												pload_P.link_state.pose.position.x)**2 +
+											 (drone_P.link_state.pose.position.y - 
+												pload_P.link_state.pose.position.y)**2 +
+											 (drone_P.link_state.pose.position.z - 
+												pload_P.link_state.pose.position.z)**2)
+					rospy.set_param('sim/tether_length',self.tetherL)
+
+				else:
+					self.tetherL = 0
+
+				self.has_run = 1
+
+			# Need to detemine their location to get angle of deflection
+			# Drone
+			drone_Px = drone_P.link_state.pose.position.x
+			drone_Py = drone_P.link_state.pose.position.y
+			drone_Pz = drone_P.link_state.pose.position.z
+
+			if self.pload == True: # If there is payload, determine the variables
+				# Pload
+				pload_Px = pload_P.link_state.pose.position.x
+				pload_Py = pload_P.link_state.pose.position.y	
+
+				# Determine theta (pitch)
+				x_sep = pload_Px - drone_Px
+
+				if math.fabs(x_sep) >= self.tetherL or x_sep == 0:
+					self.theta = 0
+				else:
+					self.theta = math.asin(x_sep/self.tetherL)
+
+				# Determine thetadot
+				self.thetadot = (self.theta - self.thetabuf)/self.dt
+				self.thetadot = self.cutoff(self.thetadot,self.thetadot_max)
+				self.thetabuf = self.theta
+			
+				# Determine phi (roll)
+				y_sep = pload_Py - drone_Py
+
+				if math.fabs(y_sep) >= self.tetherL or y_sep == 0:
+					self.phi = 0
+				else:
+					self.phi = math.asin(y_sep/self.tetherL)
+
+				# Determine phidot
+				self.phidot = (self.phi - self.phibuf)/self.dt
+				self.phidot = self.cutoff(self.phidot,self.phidot_max)
+				self.phibuf = self.phi	# Update buffer
+				
+			else: # Otherwise, vars = 0			
+				x_sep = self.phi = self.phidot = self.theta = self.thetadot = 0
+
+			# Print and save results
+			print "\n"
+			rospy.loginfo("")
+			print"Roll:           "+str(round(drone_Eul[0],2)),"\nPitch:         "+str(round(drone_Eul[1],2)),"\nYaw:           "+str(round(drone_Eul[2],2))
+			print "drone pos.x:    " + str(round(drone_Px,2))
+			print "drone pos.y:    " + str(round(drone_Py,2))
+			print "drone pos.z:    " + str(round(drone_Pz,2))
+			print "phi:	        " + str(round(self.phi*180/3.14,3))
+			print "theta:          " + str(round(self.theta*180/3.14,3))
+
+			# Populate message
+			self.status.drone_pos = drone_P.link_state.pose
+			self.status.pload_pos = pload_P.link_state.pose
+			self.status.length		= self.tetherL
+			self.status.phi 			= self.phi
+			self.status.phidot		= self.phidot
+			self.status.theta			= self.theta
+			self.status.thetadot	= self.thetadot
+
+			# Publish message
+			self.publisher.publish(self.status)			
+
+		except rospy.ServiceException as e:
+			rospy.loginfo("Get Link State call failed: {0}".format(e))
+
+	def path_follow(self,path_timer):
+		now = rospy.get_time()
+		if now - self.tstart < self.wait:
+			self.bool = False
+		else:
+			self.bool = True
+			self.pub_wd.publish(self.bool)
+				
+
+if __name__=="__main__":
+
+	# Initiate ROS node
+	rospy.init_node('linkStates_node',anonymous=False)
+	try:
+		Main() # create class object
+		rospy.spin() # loop until shutdown signal
+
+	except rospy.ROSInterruptException:
+	        pass
+

src/MoCap_Localization.py

@@ -0,0 +1,261 @@
+#!/usr/bin/env python2.7
+
+### Cesar Rodriguez Feb 2022
+### Script to determine payload and drone state using mocap
+
+import rospy,  tf
+import rosservice
+import time
+import math
+from tf.transformations import *
+from offboard_ex.msg import tethered_status
+from geometry_msgs.msg import Pose
+from gazebo_msgs.srv import GetLinkState
+from std_msgs.msg import Bool
+
+class Main:
+
+	def __init__(self):
+
+		# rate(s)
+		rate = 40 # rate for the publisher method, specified in Hz -- 20 Hz
+
+		self.dt		 = 1.0/rate
+
+		# Variables needed for testing start 
+		self.tstart		= rospy.get_time() # Keep track of the start time
+		while self.tstart == 0.0:				 # Need to make sure get_rostime works
+			self.tstart		= rospy.get_time()
+
+		### -*-*-*- Do not need this unless a test is being ran -*-*-*- ###
+		# How long should we wait before before starting test
+		#self.param_exists = False
+		#while self.param_exists == False:
+		#	if rospy.has_param('sim/wait'):
+		#		self.wait	= rospy.get_param('sim/wait') # wait time
+		#		self.param_exists = True
+		#	elif rospy.get_time() - self.tstart >= 3.0:
+		#		break
+	
+		# Will be set to true when test should start
+		#self.bool = False
+		### -*-*-*- END -*-*-*- ###
+
+		# initialize variables
+		self.phi			= 0.0 #	Payload angle of deflection from x-axis
+		self.theta		= 0.0 # Payload angle of deflection from y-axis
+		self.tetherL	= 0.0 # Tether length
+		self.has_run	= 0		# Boolean to keep track of first run instance
+		self.phidot		= 0.0 #	
+		self.thetadot	= 0.0 #
+		self.phibuf	  =	0.0 # Need buffers to determine their rates
+		self.thetabuf = 0.0 #
+		self.pload		= True # Check if payload exists
+		# Max dot values to prevent 'blowup'
+		self.phidot_max   = 3.0
+		self.thetadot_max = 3.0 
+
+		# variables for message gen
+		self.status 					= tethered_status()
+		self.status.drone_id	= 'spiri_with_tether::spiri::base'
+		self.status.drone_pos	= Pose()
+		self.status.pload_id	= 'spiri_with_tether::mass::payload'
+		self.status.pload_pos	= Pose()
+		self.status.phi				= 0.0
+		self.status.theta 		= 0.0
+
+		# service(s)
+		self.service1 = '/gazebo/get_link_state'
+
+		# need service list to check if models have spawned
+		self.service_list = rosservice.get_service_list()
+
+		# wait for service to exist
+		while self.service1 not in self.service_list:
+			print "Waiting for models to spawn..."
+			self.service_list = rosservice.get_service_list()
+			if rospy.get_time() - self.tstart >= 10.0:
+				break
+
+		# publisher(s)
+		self.publisher = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1)
+		self.pub_wd	 	 = rospy.Publisher('/status/path_follow', Bool, queue_size=1)
+
+		self.pub_timer  = rospy.Timer(rospy.Duration(1.0/rate), self.link_state)
+		self.path_timer = rospy.Timer(rospy.Duration(40.0/rate), self.path_follow)
+	
+	def cutoff(self,value,ceiling):
+		"""
+		Takes in value and returns ceiling
+		if value > ceiling. Otherwise, it returns
+		value back
+		"""
+	  # initilize sign
+		sign   = 1
+	
+		# check if value is negative
+		if value < 0.0:
+			sign = -1
+		# Cutoff value at ceiling
+		if (value > ceiling or value < -ceiling):
+			output = sign*ceiling
+		else:
+			output = value
+		return output
+
+	def euler_array(self,pose):
+		"""
+		Takes in pose msg object and outputs array of euler angs:
+			q[0] = Roll
+			q[1] = Pitch
+			q[2] = Yaw
+		"""
+		q = euler_from_quaternion([pose.orientation.x,
+															pose.orientation.y,
+															pose.orientation.z,
+															pose.orientation.w])
+		return q
+
+#	def FRD_Transform(pose)
+#		'''
+#		Transforms mocap reading to proper coordinate frame
+#		'''
+#		pose.position.x = 
+#		pose.position.y =
+#		pose.position.z =
+
+#		Keep the w same and change x, y, and z as above.
+#		pose.orientation.x =
+#		pose.orientation.y =
+#		pose.orientation.z =
+#		pose.orientation.w =
+ 
+#		return pose
+
+	# Get link states (drone and pload) and determine angle between them
+	def link_state(self,pub_timer):
+		
+		try:
+
+			# State which service we are querying
+			get_P = rospy.ServiceProxy(self.service1,GetLinkState)
+
+			# Set reference frame
+			reference	= ''
+
+			# Establish links needed --> Spiri base and payload
+			# P = Position vector
+			drone_P = get_P(self.status.drone_id,reference)
+
+			# Get orientation of drone in euler angles
+			drone_Eul = self.euler_array(drone_P.link_state.pose)
+
+			# Check if payload is part of simulation
+			if not drone_P.success:
+				self.status.drone_id	= 'spiri::base'
+				drone_P = get_P(self.status.drone_id,reference) # i.e. no payload
+				self.pload = False
+
+			pload_P = get_P(self.status.pload_id,reference)
+
+			if not self.has_run == 1:
+				if self.pload == True:
+					# Get tether length based off initial displacement
+					self.tetherL = math.sqrt((drone_P.link_state.pose.position.x - 
+												pload_P.link_state.pose.position.x)**2 +
+											 (drone_P.link_state.pose.position.y - 
+												pload_P.link_state.pose.position.y)**2 +
+											 (drone_P.link_state.pose.position.z - 
+												pload_P.link_state.pose.position.z)**2)
+					rospy.set_param('sim/tether_length',self.tetherL)
+
+				else:
+					self.tetherL = 0
+
+				self.has_run = 1
+
+			# Need to detemine their location to get angle of deflection
+			# Drone
+			drone_Px = drone_P.link_state.pose.position.x
+			drone_Py = drone_P.link_state.pose.position.y
+			drone_Pz = drone_P.link_state.pose.position.z
+
+			if self.pload == True: # If there is payload, determine the variables
+				# Pload
+				pload_Px = pload_P.link_state.pose.position.x
+				pload_Py = pload_P.link_state.pose.position.y	
+
+				# Determine theta (pitch)
+				x_sep = pload_Px - drone_Px
+
+				if math.fabs(x_sep) >= self.tetherL or x_sep == 0:
+					self.theta = 0
+				else:
+					self.theta = math.asin(x_sep/self.tetherL)
+
+				# Determine thetadot
+				self.thetadot = (self.theta - self.thetabuf)/self.dt
+				self.thetadot = self.cutoff(self.thetadot,self.thetadot_max)
+				self.thetabuf = self.theta
+			
+				# Determine phi (roll)
+				y_sep = pload_Py - drone_Py
+
+				if math.fabs(y_sep) >= self.tetherL or y_sep == 0:
+					self.phi = 0
+				else:
+					self.phi = math.asin(y_sep/self.tetherL)
+
+				# Determine phidot
+				self.phidot = (self.phi - self.phibuf)/self.dt
+				self.phidot = self.cutoff(self.phidot,self.phidot_max)
+				self.phibuf = self.phi	# Update buffer
+				
+			else: # Otherwise, vars = 0			
+				x_sep = self.phi = self.phidot = self.theta = self.thetadot = 0
+
+			# Print and save results
+			print "\n"
+			rospy.loginfo("")
+			print"Roll:           "+str(round(drone_Eul[0],2)),"\nPitch:         "+str(round(drone_Eul[1],2)),"\nYaw:           "+str(round(drone_Eul[2],2))
+			print "drone pos.x:    " + str(round(drone_Px,2))
+			print "drone pos.y:    " + str(round(drone_Py,2))
+			print "drone pos.z:    " + str(round(drone_Pz,2))
+			print "phi:	        " + str(round(self.phi*180/3.14,3))
+			print "theta:          " + str(round(self.theta*180/3.14,3))
+
+			# Populate message
+			self.status.drone_pos = drone_P.link_state.pose
+			self.status.pload_pos = pload_P.link_state.pose
+			self.status.length		= self.tetherL
+			self.status.phi 			= self.phi
+			self.status.phidot		= self.phidot
+			self.status.theta			= self.theta
+			self.status.thetadot	= self.thetadot
+
+			# Publish message
+			self.publisher.publish(self.status)			
+
+		except rospy.ServiceException as e:
+			rospy.loginfo("Get Link State call failed: {0}".format(e))
+
+#  def path_follow(self,path_timer):
+#		now = rospy.get_time()
+#		if now - self.tstart < self.wait:
+#			self.bool = False
+#		else:
+#			self.bool = True
+#			self.pub_wd.publish(self.bool)
+				
+
+if __name__=="__main__":
+
+	# Initiate ROS node
+	rospy.init_node('linkStates_node',anonymous=False)
+	try:
+		Main() # create class object
+		rospy.spin() # loop until shutdown signal
+
+	except rospy.ROSInterruptException:
+	        pass
+

src/offb_node.cpp

@@ -0,0 +1,172 @@
+/**
+ * @file offb_node.cpp
+ * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
+ * Stack and tested in Gazebo SITL
+ * Cesar: Now used for path following on topic \path 
+ */
+
+#include <ros/ros.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Quaternion.h>
+#include <mavros_msgs/CommandBool.h>
+#include <mavros_msgs/SetMode.h>
+#include <mavros_msgs/State.h>
+#include <mavros_msgs/CommandTOL.h>
+#include <mavros_msgs/AttitudeTarget.h>
+#include <mavros_msgs/Thrust.h>
+#include <mavros_msgs/VFR_HUD.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <iostream>
+
+
+/********* CALLBACK FUNCTIONS **********************/
+// Initiate variables
+mavros_msgs::State current_state;
+geometry_msgs::PoseStamped desPose;
+
+// Callback function which will save the current state of the autopilot. 
+// Allows to check connection, arming, and Offboard tags*/
+void state_cb(const mavros_msgs::State::ConstPtr& msg){
+    current_state = *msg;
+}
+
+// Cb to recieve pose information
+// Initiate variables
+geometry_msgs::PoseStamped pose;
+geometry_msgs::Quaternion q_init;
+geometry_msgs::PoseStamped mavPose;
+bool pose_read = false;
+double current_altitude;
+
+void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
+	mavPose = *msg;
+	current_altitude = mavPose.pose.position.z;
+	while(pose_read == false){
+		q_init = mavPose.pose.orientation;
+		if(q_init.x == 0.0 && q_init.w == 0.0){
+			ROS_INFO("Waiting...");
+		} else {
+			pose_read = true;
+		    	pose.pose.orientation.x = q_init.x;
+   			pose.pose.orientation.y = q_init.y;
+    			pose.pose.orientation.z = q_init.z;
+    			pose.pose.orientation.w = q_init.w;
+		}
+	}
+
+}
+
+// Determine new waypoins
+geometry_msgs::PoseStamped wpoints;
+bool des_waypoints = true;
+void waypoints_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
+	wpoints = *msg;
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "offb_node");
+    ros::NodeHandle nh;
+    
+    /********************** SUBSCRIBERS **********************/
+    // Get current state
+    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
+            ("mavros/state", 10, state_cb);
+
+    // Pose subscriber
+    ros::Subscriber mavPose_sub = nh.subscribe<geometry_msgs::PoseStamped>
+	("mavros/local_position/pose",10,mavPose_cb);
+
+    // Waypoint Subscriber
+    ros::Subscriber waypoint_sub = nh.subscribe<geometry_msgs::PoseStamped>
+	("/reference/waypoints",10,waypoints_cb);
+
+    /********************** PUBLISHERS **********************/
+    // Initiate publisher to publish commanded local position
+    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
+            ("mavros/setpoint_position/local", 10);
+
+    // Publish desired attitude
+    ros::Publisher thrust_pub = nh.advertise<mavros_msgs::Thrust>
+            ("mavros/setpoint_attitude/thrust", 10);
+
+    // Publish attitude commands
+    ros::Publisher att_pub = nh.advertise<geometry_msgs::PoseStamped>
+	    ("/mavros/setpoint_attitude/attitude",10);
+
+    // Service Clients
+    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
+            ("mavros/cmd/arming");
+    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
+            ("mavros/set_mode");
+    ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
+	    ("mavros/cmd/takeoff");
+
+    //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
+    ros::Rate rate(20.0);
+
+    // wait for FCU connection
+    while(ros::ok() && !current_state.connected){
+        ros::spinOnce();
+        rate.sleep();
+    }
+   
+    // Populate pose msg
+    pose.pose.position.x = 0;
+    pose.pose.position.y = 0;
+    pose.pose.position.z = 5.0;
+    pose.pose.orientation.x = q_init.x;
+    pose.pose.orientation.y = q_init.y;
+    pose.pose.orientation.z = q_init.z;
+    pose.pose.orientation.w = q_init.w;
+
+
+    //send a few setpoints before starting
+    for(int i = 100; ros::ok() && i > 0; --i){
+        local_pos_pub.publish(pose);
+        ros::spinOnce();
+        rate.sleep();
+    }
+
+    mavros_msgs::SetMode offb_set_mode;
+    offb_set_mode.request.custom_mode = "OFFBOARD";
+
+    mavros_msgs::CommandBool arm_cmd;
+    arm_cmd.request.value = true;
+
+    ros::Time last_request = ros::Time::now();
+
+    while(ros::ok()){
+        if(current_state.mode != "OFFBOARD" &&
+            (ros::Time::now() - last_request > ros::Duration(5.0))){
+            if( set_mode_client.call(offb_set_mode) &&
+                offb_set_mode.response.mode_sent){
+                ROS_INFO("Offboard enabled");
+            }
+            last_request = ros::Time::now();
+        } else {
+            if( !current_state.armed &&
+                (ros::Time::now() - last_request > ros::Duration(3.0))){
+                if(arming_client.call(arm_cmd) &&
+                    arm_cmd.response.success){
+                    ROS_INFO("Vehicle armed");
+                }
+                last_request = ros::Time::now();
+            }
+        }
+		if(des_waypoints == true){
+			pose.pose.position.x = wpoints.pose.position.x;
+			pose.pose.position.y = wpoints.pose.position.y;
+		}
+		
+		local_pos_pub.publish(pose);
+
+		ros::spinOnce();
+		rate.sleep();
+		}
+
+    return 0;
+}
+
+

src/path_follow.cpp

@@ -0,0 +1,252 @@
+/**
+ * @file path_follow.cpp
+ * @brief Offboard path trajectory tracking
+*/
+
+#include <ros/ros.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <offboard_ex/RefSignal.h>
+#include <offboard_ex/EulerAngles.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/Vector3.h>
+#include <mavros_msgs/CommandBool.h>
+#include <mavros_msgs/SetMode.h>
+#include <mavros_msgs/State.h>
+#include <mavros_msgs/CommandTOL.h>
+#include <mavros_msgs/AttitudeTarget.h>
+#include <mavros_msgs/Thrust.h>
+#include <mavros_msgs/VFR_HUD.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <iostream>
+#include <cmath> // for std::abs
+
+/********* CALLBACK FUNCTIONS **********************/
+// Initiate variables
+mavros_msgs::State current_state;
+
+geometry_msgs::PoseStamped desPose;
+
+tf2::Quaternion q_cmd /*What I send*/, q_Jae; // What should be sent;
+double rol_K, pch_K, yaw_K, rol_J, pch_J, yaw_J; // Same logic, K = cmd
+geometry_msgs::Vector3 K_ang, J_ang; // Needed to populate EulerAngle msg
+offboard_ex::EulerAngles eulAng;
+
+mavros_msgs::AttitudeTarget thrust;
+
+// Callback function which will save the current state of the autopilot. 
+// Allows to check connection, arming, and Offboard tags*/
+void state_cb(const mavros_msgs::State::ConstPtr& msg){
+    current_state = *msg;
+}
+
+geometry_msgs::Quaternion q_des;
+double alt_des; // Check desired height
+
+// Cb to determine desired pose *Only needed for orientation 
+void q_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
+    desPose = *msg;
+		alt_des = desPose.pose.position.z;
+    q_des   = desPose.pose.orientation;
+    tf2::convert(q_des,q_cmd);	// Convert msg type to tf2::quaternion type
+}
+
+
+// Cb to recieve pose information
+geometry_msgs::PoseStamped buff_pose;
+geometry_msgs::Quaternion q_init;
+geometry_msgs::PoseStamped mavPose;
+bool gps_read = false;
+double current_altitude;
+
+void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
+	mavPose = *msg;
+	current_altitude = mavPose.pose.position.z;
+	while(gps_read == false){
+		q_init = mavPose.pose.orientation;
+		if(q_init.x == 0.0 && q_init.w == 0.0){
+			ROS_INFO("Waiting...");
+		} else {
+			gps_read = true;
+		    	buff_pose.pose.orientation.x = q_init.x;
+   				buff_pose.pose.orientation.y = q_init.y;
+    			buff_pose.pose.orientation.z = q_init.z;
+    			buff_pose.pose.orientation.w = q_init.w;
+		}
+	}
+}
+
+// To determine throttle ... type
+void thrust_cb(const mavros_msgs::AttitudeTarget msg){
+	thrust = msg;	
+	tf2::convert(thrust.orientation,q_Jae); // Convert from msg to quat
+}
+
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "path_follow");
+    ros::NodeHandle nh;
+    
+    /********************** SUBSCRIBERS **********************/
+    // Get current state
+    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
+		("mavros/state", 10, state_cb);
+
+    // Get quaternions from klausen control
+    ros::Subscriber q_sub = nh.subscribe<geometry_msgs::PoseStamped>
+		("command/quaternions",10,q_cb);
+
+    // Pose subscriber
+    ros::Subscriber mavPose_sub = nh.subscribe<geometry_msgs::PoseStamped>
+		("mavros/local_position/pose",10,mavPose_cb);
+
+    // Throttle: Jaeyoung-Lim
+    ros::Subscriber thro_sub = nh.subscribe<mavros_msgs::AttitudeTarget>
+		("command/bodyrate_command",10,thrust_cb);
+
+    /********************** PULISHERS **********************/
+    // Initiate publisher to publish commanded local position
+    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
+		("mavros/setpoint_position/local", 10);
+
+    // Publish attitude and thrust commands
+    ros::Publisher att_targ = nh.advertise<mavros_msgs::AttitudeTarget>
+		("mavros/setpoint_raw/attitude",10);
+
+    // Publish attitude cmds in euler angles
+    ros::Publisher euler_pub = nh.advertise<offboard_ex::EulerAngles>
+		("command/euler_angles",10);
+
+    // Service Clients
+    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
+		("mavros/cmd/arming");
+    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
+    ("mavros/set_mode");
+    ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
+		("mavros/cmd/takeoff");
+
+    //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
+    ros::Rate rate(20.0);
+
+    // wait for FCU connection
+    while(ros::ok() && !current_state.connected){
+        ros::spinOnce();
+	ROS_INFO("Waiting for FCU connection");
+        rate.sleep();
+    }
+   
+    /*********** Initiate variables ************************/
+    // Needed for attitude command
+    mavros_msgs::AttitudeTarget attitude;
+    
+    attitude.header.frame_id = "map"; // NED Ref Frame(?) #TODO 
+    attitude.type_mask = 1|2|4; // Ignore body rates
+
+    // Need to send pose commands until throttle has been established
+    buff_pose.pose.position.x = 0.0;
+    buff_pose.pose.position.y = 0.0;
+    buff_pose.pose.position.z = 4.5;
+
+    // Desired mode is offboard
+    mavros_msgs::SetMode offb_set_mode;
+    offb_set_mode.request.custom_mode = "OFFBOARD";
+
+    // Arm UAV
+    mavros_msgs::CommandBool arm_cmd;
+    arm_cmd.request.value = true;
+
+    // Take off command    
+    bool takeoff = false, att_cmds = false;
+
+    // Keep track of time since requests
+    ros::Time tkoff_req = ros::Time::now();
+    ros::Time last_request = ros::Time::now();
+
+    //send a few setpoints before starting
+    for(int i = 100; ros::ok() && i > 0; --i){
+        local_pos_pub.publish(buff_pose);
+        ros::spinOnce();
+        rate.sleep();
+    }
+
+    while(ros::ok()){
+		if(gps_read == true){
+		// Now need to convert publish q to euler ang
+		tf2::Matrix3x3 m_K(q_cmd); tf2::Matrix3x3 m_J(q_Jae);
+		m_K.getRPY(rol_K, pch_K, yaw_K);
+		m_J.getRPY(rol_J, pch_J, yaw_J);
+
+		// Populate msg
+		K_ang.x = 180*rol_K/3.141;
+		K_ang.y = 180*pch_K/3.141;
+		K_ang.z = yaw_K;
+
+		J_ang.x = rol_J;
+		J_ang.y = pch_J;
+		J_ang.z = yaw_J;
+
+		eulAng.commanded = K_ang;
+		eulAng.desired = J_ang;
+
+		if(current_state.mode != "OFFBOARD" &&
+		    (ros::Time::now() - last_request > ros::Duration(5.0))){
+		    if( set_mode_client.call(offb_set_mode) &&
+		        offb_set_mode.response.mode_sent){
+		        ROS_INFO("Offboard enabled");
+		    } else {
+			ROS_INFO("Could not enter offboard mode");
+		    }
+		    last_request = ros::Time::now();
+		} else {
+		    if( !current_state.armed &&
+		        (ros::Time::now() - last_request > ros::Duration(3.0))){
+		        if( arming_client.call(arm_cmd) &&
+		            arm_cmd.response.success){
+		            ROS_INFO("Vehicle armed");
+		        }
+		        last_request = ros::Time::now();
+		    }
+		}
+		
+		if(current_state.mode == "OFFBOARD" && current_state.armed){
+			ROS_INFO_ONCE("Spiri is taking off");
+			if(!takeoff){		
+				tkoff_req = ros::Time::now();
+				takeoff = true;
+			}
+		}
+		attitude.thrust = thrust.thrust;
+		
+		// Determine which messages to send
+		if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff){
+			attitude.orientation = q_des;
+			attitude.header.stamp = ros::Time::now();
+			att_targ.publish(attitude);
+			att_cmds = true;	
+		} else {
+			local_pos_pub.publish(buff_pose);
+		}
+
+		if(!att_cmds && ros::Time::now() - last_request > ros::Duration(2.0)){
+			ROS_INFO("Thro: %.2f", attitude.thrust);
+		} else {
+			ROS_INFO("Thro: %.2f", attitude.thrust);
+			ROS_INFO("Des Altitude: %.2f", alt_des);
+			ROS_INFO("Cur Altitude: %.2f", current_altitude);
+			//ROS_INFO("Sending points:\nRoll = %.2f\nPitch = %.2f",
+			//	  180*rol_K/3.141,180*pch_K/3.141);
+		}
+
+		// Publish euler angs
+		euler_pub.publish(eulAng);
+		
+		ros::spinOnce();
+		rate.sleep();
+	}
+    }
+
+    return 0;
+}
+

src/quarternion.py

@@ -0,0 +1,48 @@
+#!/usr/bin/env python
+
+import rospy,  tf
+import numpy as np
+import time
+import math
+
+from tf.transformations import *
+from geometry_msgs.msg import PoseStamped, Quaternion
+
+class Main:
+	def __init__(self):
+
+		# rate
+		rate = 40
+		self.Euler = [0,0,1.571]
+		self.pose = PoseStamped()
+		self.q = [0,0,0,0]
+
+		self.pub_q = rospy.Publisher('/quaternions',PoseStamped,queue_size = 10)
+		self.pub_time = rospy.Timer(rospy.Duration(1.0/rate),self.publisher)
+
+	def publisher(self,pub_tim):
+		q = quaternion_from_euler(self.Euler[0],self.Euler[1],self.Euler[2])
+		#q_msg = Quaternion(q[0],q[1],q[2],q[3])
+
+		self.pose.header.stamp = rospy.Time.now()
+		self.pose.pose.position.x = 0
+		self.pose.pose.position.y = 0
+		self.pose.pose.position.z = 2.5
+		self.pose.pose.orientation.x = q[0] 
+		self.pose.pose.orientation.y = q[1]
+		self.pose.pose.orientation.z = q[2]
+		self.pose.pose.orientation.w = q[3]
+
+		self.pub_q.publish(self.pose)
+		
+		rospy.loginfo("Fly to altitude: %.2f m",self.pose.pose.position.z)
+
+if __name__=="__main__":
+	rospy.init_node('quart_node')
+	try:
+		Main()
+		rospy.spin()
+
+	except rospy.ROSInterruptException:
+		pass
+