Mavros Controllers is no longer a dependency of this package

CMakeLists.txt

@@ -13,7 +13,6 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
   message_runtime
   roscpp
-  controller_msgs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -77,7 +76,6 @@ generate_messages(
   std_msgs
   geometry_msgs
   sensor_msgs
-  controller_msgs
 )
 
 ################################################

config/noCtrl_param.yaml

@@ -1,4 +1,4 @@
 # Ros param when not using Klausen Ctrl
 
 wait: 55 
-waypoints: {x:  0.0, y:  0.0, z:  3.0}
+waypoints: {x:  0.0, y:  0.0, z:  5.0}

launch/oscillation_damp.launch

@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<!--
+Launch file to use klausen oscillaton damping ctrl in Gazebo
+/-->
+<launch>
+  <arg name="model" default="headless_spiri_with_tether"/>
+  <arg name='test' default="none"/>
+
+  <group ns="sim">
+  	<rosparam file="$(find oscillation_ctrl)/config/Ctrl_param.yaml" />
+  </group>
+
+  <group ns="status">
+  	<param name="test_type" value="$(arg test)"/>
+  </group>
+
+	<!-- LOCALIZES DRONE & DETERMINES LOAD ANGLES -->
+	<node
+		pkg="oscillation_ctrl"
+		type="LinkState.py"
+		name="linkStates_node"
+		launch-prefix="xterm -e"
+	/>
+	<!-- DETERMINES DESIRED POSITION -->
+	<node
+		pkg="oscillation_ctrl"
+		type="wpoint_tracker.py"
+		name="waypoints_server"
+	/>
+	<!-- CREATES DESIRED TRAJECTORY/ REFERENCE SIGNAL -->
+	<node
+		pkg="oscillation_ctrl"
+		type="ref_signalGen.py"
+		name="refSignal_node"
+	/>
+	<!-- DETERMINES DESIRED ATTITUDE AND THRUST BASED ON REF. SIG. -->
+	<node
+		pkg="oscillation_ctrl"
+		type="klausen_control.py"
+		name="klausenCtrl_node"
+		output="screen"
+	/>
+	<!-- PUBLISHES DESIRED COMMANDS -->
+	<node
+		pkg="oscillation_ctrl"
+		type="pathFollow_node"
+		name="pathFollow_node"
+		launch-prefix="xterm -e"
+	/>
+	
+	<!-- RUNS DIFFERENT TESTS IF DESIRED -->
+	<group if="$(eval test != 'none')">
+		<node
+			pkg="oscillation_ctrl"
+			type="perform_test.py"
+			name="test_node"
+			launch-prefix="xterm -e"
+		/>
+	</group>
+	<!-- PX4 LAUNCH -->
+	<include file="$(find px4)/launch/$(arg model).launch"/>
+</launch>

launch/takeoff_noCtrl.launch

@@ -1,10 +1,13 @@
 <?xml version="1.0"?>
 <launch>
-  <arg name="test_type" default="step.py" />
+  <arg name="model" default="headless_spiri_with_tether"/>
-  <arg name="model" default="spiri"/>
+  <arg name='test' default="none"/>
   <group ns="sim">
   	<rosparam file="$(find oscillation_ctrl)/config/noCtrl_param.yaml" />
   </group>
+  <group ns="status">
+  	<param name="test_type" value="$(arg test)"/>
+  </group>
 
 	<node
 		pkg="oscillation_ctrl"
@@ -18,13 +21,22 @@
 		name="offb_node"
 		launch-prefix="xterm -e"
 	/>
+
+
 	<node
 		pkg="oscillation_ctrl"
-		type="$(arg test_type)"
+		type="wpoint_tracker.py"
-		name="test_node"
+		name="waypoints_server"
-		launch-prefix="xterm -e"
 	/>
 
+	<group if="$(eval test != 'none')">
+		<node
+			pkg="oscillation_ctrl"
+			type="perform_test.py"
+			name="test_node"
+			launch-prefix="xterm -e"
+		/>
+	</group>
 	<!-- PX4 LAUNCH -->
 	<include file="$(find px4)/launch/$(arg model).launch"/>
 </launch>

msg/RefSignal.msg

@@ -1,7 +1,7 @@
 # Contains array of smooth path signal at a given time
 # ref sign contains des pos, vel, and acc
 # 
-#std_msgs/Header header			# Timestamp
+std_msgs/Header header			# Timestamp
 geometry_msgs/Vector3 position		# Desired pos
 geometry_msgs/Vector3 velocity		# Desired vel
 geometry_msgs/Vector3 acceleration	# Desired acc

src/LinkState.py

@@ -207,7 +207,9 @@ class Main:
 		# Print and save results
 		#print "\n"
 		rospy.loginfo("")
-		print"Roll:           "+str(round(self.drone_Eul[0]*180/3.14,2)),"\nPitch:         "+str(round(self.drone_Eul[1]*180/3.14,2)),"\nYaw:           "+str(round(self.drone_Eul[2]*180/3.14,2))
+		print"Roll:           "+str(round(self.drone_Eul[0]*180/3.14,2))
+		print"Pitch:         " +str(round(self.drone_Eul[1]*180/3.14,2))
+		print"Yaw:           " +str(round(self.drone_Eul[2]*180/3.14,2))
 		print "drone pos.x:    " + str(round(self.drone_Px,2))
 		print "drone pos.y:    " + str(round(self.drone_Py,2))
 		print "drone pos.z:    " + str(round(self.drone_Pz,2))
@@ -231,4 +233,4 @@ if __name__=="__main__":
 		rospy.spin() # loop until shutdown signal
 
 	except rospy.ROSInterruptException:
-	        pass
+	        pass

src/klausen_control.py

@@ -14,13 +14,11 @@ import math
 
 from tf.transformations   import *
 from scipy.integrate  	  import odeint 
-from oscillation_ctrl.msg import tethered_status, RefSignal, LoadAngles
+from oscillation_ctrl.msg import RefSignal, LoadAngles
-from controller_msgs.msg  import FlatTarget
+from oscillation_ctrl.srv import WaypointTrack
-from geometry_msgs.msg 	  import Point, Pose
+from geometry_msgs.msg 	  import Pose, Point, TwistStamped, PoseStamped
-from geometry_msgs.msg 	  import TwistStamped, Vector3, Vector3Stamped, PoseStamped, Quaternion
-from gazebo_msgs.srv 	  import GetLinkState
 from sensor_msgs.msg 	  import Imu
-from pymavlink 		 	  import mavutil
+from mavros_msgs.msg 	  import AttitudeTarget
 
 class Main:
 
@@ -42,20 +40,24 @@ class Main:
 		
 		# Msgs types
 		self.vel_data	 = TwistStamped() # This is needed to get drone vel from gps
-		self.imu_data	 =	Imu() 				 # Needed for to get drone acc from IMU
+		self.imu_data	 = Imu() 				 # Needed for to get drone acc from IMU
 		self.path_pos	 = np.zeros([3,1])
-		self.path_vel	 =	np.zeros([3,1]) 
+		self.path_vel	 = np.zeros([3,1]) 
 		self.path_acc 	 = np.zeros([3,1]) 
 		self.dr_pos 	 = Pose()
-		self.quaternion  = PoseStamped()	# used to send quaternion attitude commands
+		self.att_targ    = AttitudeTarget()	# used to send quaternion attitude commands
 		self.load_angles = LoadAngles()
 		self.EulerAng	 = [0,0,0] # Will find the euler angles, and then convert to q
 
+		# Service var
+		self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)		
+		self.empty_point = Point() # Needed to query waypoint_server
+
 		# Drone var
 		self.has_run	 = 0		# Bool to keep track of first run instance
 
 		# Col1 = theta, theta dot; Col2 = phi, phidot for self.PHI
-		self.PHI 		=	np.array([[0,0],[0,0]]) 
+		self.PHI 		= np.array([[0,0],[0,0]]) 
 		self.dr_vel		= np.zeros([3,1])
 		self.dr_angVel	= np.zeros([3,1])
 		self.dr_acc		= np.zeros([3,1]) #self.imu_data.linear_acceleration
@@ -93,33 +95,41 @@ class Main:
 		self.drone_m  = 1.437
 		self.pl_m	  =	0.5
 		self.tot_m 	  = self.drone_m + self.pl_m
+
 		self.tune	  = 1 # Tuning parameter
 		self.dist	  =	np.array([0,0,0,0.01,0.01]) # Wind disturbance
 		
+		# PD Thrust Controller terms
+		# gains for thrust PD Controller
+		self.Kp 	= 2.7 
+		self.Kd 	= 3 #2
+		self.max_a	= 14.2
+		self.max_t  = self.drone_m*self.max_a
+		self.R 		= np.empty([3,3]) # rotation matrix
+		self.e3 = np.array([[0],[0],[1]])
+		self.thrust_offset = 0.7	# There was found to be a constant offset
 # --------------------------------------------------------------------------------#		
-# 																	SUBSCRIBERS
+# 									SUBSCRIBERS
 # --------------------------------------------------------------------------------#
 		# Topic, msg type, and class callback method
 		rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb)
-		rospy.Subscriber('/reference/path', FlatTarget, self.refsig_cb)
+		rospy.Subscriber('/reference/path', RefSignal, self.refsig_cb)
 		#rospy.Subscriber('/status/twoBody_status', tethered_status, self.dronePos_cb)
 		rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb)
 		rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb)
 		rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
 		
 # --------------------------------------------------------------------------------#		
-# 																	PUBLISHERS
+# 									PUBLISHERS
 # --------------------------------------------------------------------------------#
 
-		self.pub_q = rospy.Publisher('/command/quaternions',PoseStamped,queue_size = 10)
+		self.pub_att_targ = rospy.Publisher('/command/att_target',AttitudeTarget,queue_size = 10)
 
 		# timer(s), used to control method loop freq(s) as defined by the rate(s)
 		self.pub_time  = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) #f this was 5.0 rate before
-		
-# ------------------------------------ _init_ ends ------------------------------ # 
 
 # --------------------------------------------------------------------------------#		
-# 																CALLBACK FUNCTIONS
+# 								CALLBACK FUNCTIONS
 # --------------------------------------------------------------------------------#
 
 	# Callback pload deflection angles and vel
@@ -137,7 +147,7 @@ class Main:
 	# Callback drone pose
 	def dronePos_cb(self,msg): 
 		try:
-			self.dr_pos 	= msg.pose
+			self.dr_pos = msg.pose
 			#self.dr_pos = msg.drone_pos
 
 		except ValueError:
@@ -177,6 +187,13 @@ class Main:
 		else:
 			rospy.loginfo_once('NO TETHER DETECTED')
 
+	def waypoints_srv_cb(self):
+		rospy.wait_for_service('/status/waypoint_tracker')
+		try:
+			resp = self.get_xd(False,self.empty_point)
+			self.xd = resp.xd
+		except ValueError:
+			pass
 
 # ---------------------------------ODE SOLVER-------------------------------------#
 	def statespace(self,y,t,Ka,Kb,Kc):
@@ -190,8 +207,66 @@ class Main:
 		
 		return dydt
 # --------------------------------------------------------------------------------#
+	def quaternion_rotation_matrix(self):
+		"""
+		Covert a quaternion into a full three-dimensional rotation matrix.
 
-	def control(self):
+		Input
+		:param Q: A 4 element array representing the quaternion (q0,q1,q2,q3) 
+
+		Output
+		:return: A 3x3 element matrix representing the full 3D rotation matrix. 
+		         This rotation matrix converts a point in the local reference 
+		         frame to a point in the global reference frame.
+		"""
+		# Extract the values from Q
+		q0 = self.dr_pos.orientation.w
+		q1 = self.dr_pos.orientation.x
+		q2 = self.dr_pos.orientation.y
+		q3 = self.dr_pos.orientation.z
+		 
+		# First row of the rotation matrix
+		r00 = 2 * (q0 * q0 + q1 * q1) - 1
+		r01 = 2 * (q1 * q2 - q0 * q3)
+		r02 = 2 * (q1 * q3 + q0 * q2)
+		 
+		# Second row of the rotation matrix
+		r10 = 2 * (q1 * q2 + q0 * q3)
+		r11 = 2 * (q0 * q0 + q2 * q2) - 1
+		r12 = 2 * (q2 * q3 - q0 * q1)
+		 
+		# Third row of the rotation matrix
+		r20 = 2 * (q1 * q3 - q0 * q2)
+		r21 = 2 * (q2 * q3 + q0 * q1)
+		r22 = 2 * (q0 * q0 + q3 * q3) - 1
+		 
+		# 3x3 rotation matrix
+		rot_matrix = np.array([[r00, r01, r02],
+		                       [r10, r11, r12],
+		                       [r20, r21, r22]])
+		                        
+		return rot_matrix
+
+	def determine_throttle(self):
+		# thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3)
+		# Taeyoung Lee, Melvin Leok, and N. Harris McClamroch
+		self.waypoints_srv_cb()
+		self.error = np.array([[self.dr_pos.position.x - self.xd.x],
+							[self.dr_pos.position.y - self.xd.y],
+							[self.dr_pos.position.z - self.xd.z - self.thrust_offset]]) 
+
+		self.R = self.quaternion_rotation_matrix()	# determine Rotation Matrix 
+
+
+		#thrust = np.dot(np.dot(9.81*self.drone_m,self.e3) - self.Kp*self.error - self.Kd*self.dr_vel.T,self.R.T[2])/self.max_t
+		thrust_vector = np.dot(np.dot(9.81*self.drone_m,self.e3) - self.Kp*self.error - self.Kd*self.dr_vel.T,self.R.T[2])/self.max_t
+		thrust = thrust_vector[2]
+
+		# Value needs to be between 0 - 1.0
+		self.att_targ.thrust = max(0.0,min(thrust,1.0))
+
+
+	def determine_q(self):
 		# Populate position vector and gamma (g). g is state space vector: [px,py,pz,theta,phi]
 		p =	np.array([[self.dr_pos.position.x],[self.dr_pos.position.y],[self.dr_pos.position.z]])
 		g = np.array([[self.dr_pos.position.x],[self.dr_pos.position.y],[self.dr_pos.position.z],[self.load_angles.theta],[self.load_angles.phi]])
@@ -305,17 +380,19 @@ class Main:
 		q = quaternion_from_euler(self.EulerAng[0],self.EulerAng[1],self.EulerAng[2])
 
 		# Populate msg variable
-
 		# Attitude control
-		self.quaternion.header.stamp = rospy.Time.now()
+		self.att_targ.header.stamp = rospy.Time.now()
-		self.quaternion.pose.orientation.x = q[0] 
+		self.att_targ.header.frame_id = 'map'
-		self.quaternion.pose.orientation.y = q[1]
+		self.att_targ.type_mask = 1|2|4 
-		self.quaternion.pose.orientation.z = q[2]
+		self.att_targ.orientation.x = q[0] 
-		self.quaternion.pose.orientation.w = q[3]
+		self.att_targ.orientation.y = q[1]
+		self.att_targ.orientation.z = q[2]
+		self.att_targ.orientation.w = q[3]
 		
 	def publisher(self,pub_time):
-		self.control()
+		self.determine_q()
-		self.pub_q.publish(self.quaternion)
+		self.determine_throttle()
+		self.pub_att_targ.publish(self.att_targ)
 		#rospy.loginfo('roll: %.2f\npitch: %.2f\n',self.EulerAng[0],self.EulerAng[1])
 
 # --------------------------------------------------------------------------------#		
@@ -328,7 +405,7 @@ if __name__=="__main__":
 	# Initiate ROS node
 	rospy.init_node('trajectoryCtrl',anonymous=True)
 	try:
-		Main() 				# create class object
+		Main() 			# create class object
 		rospy.spin() 	# loop until shutdown signal
 
 	except rospy.ROSInterruptException:

src/offb_node.cpp

@@ -15,6 +15,7 @@
 #include <mavros_msgs/AttitudeTarget.h>
 #include <mavros_msgs/Thrust.h>
 #include <mavros_msgs/VFR_HUD.h>
+#include <oscillation_ctrl/WaypointTrack.h>
 #include <tf2/LinearMath/Quaternion.h>
 #include <iostream>
 
@@ -56,12 +57,6 @@ void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
 
 }
 
-// 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)
 {
@@ -78,17 +73,21 @@ int main(int argc, char **argv)
 	("mavros/local_position/pose",10,mavPose_cb);
 
     // Waypoint Subscriber
+    /*
     ros::Subscriber waypoint_sub = nh.subscribe<geometry_msgs::PoseStamped>
 	("/reference/waypoints",10,waypoints_cb);
-
+    */
+    ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
+	("status/waypoint_tracker");
+    
     /********************** PUBLISHERS **********************/
     // Initiate publisher to publish commanded local position
     ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
-            ("mavros/setpoint_position/local", 10);
+        ("mavros/setpoint_position/local", 10);
 
     // Publish desired attitude
     ros::Publisher thrust_pub = nh.advertise<mavros_msgs::Thrust>
-            ("mavros/setpoint_attitude/thrust", 10);
+        ("mavros/setpoint_attitude/thrust", 10);
 
     // Publish attitude commands
     ros::Publisher att_pub = nh.advertise<geometry_msgs::PoseStamped>
@@ -96,9 +95,9 @@ int main(int argc, char **argv)
 
     // Service Clients
     ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
-            ("mavros/cmd/arming");
+        ("mavros/cmd/arming");
     ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
-            ("mavros/set_mode");
+        ("mavros/set_mode");
     ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
 	    ("mavros/cmd/takeoff");
 
@@ -110,16 +109,26 @@ int main(int argc, char **argv)
         ros::spinOnce();
         rate.sleep();
     }
-   
+
-    // Populate pose msg
+   	// Retrieve desired waypoints
-    pose.pose.position.x = 0;
+	oscillation_ctrl::WaypointTrack wpoint_srv;
-    pose.pose.position.y = 0;
+	wpoint_srv.request.query = false;
-    pose.pose.position.z = 1.5;
+	if (waypoint_cl.call(wpoint_srv))
+	{
+		ROS_INFO("Waypoints received");
+	}
+	
+	// populate desired waypoints
+    pose.pose.position.x = wpoint_srv.response.xd.x;
+    pose.pose.position.y = wpoint_srv.response.xd.y;
+    pose.pose.position.z = wpoint_srv.response.xd.z;
+
+    /*/ Populate pose msg
     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){
@@ -154,10 +163,12 @@ int main(int argc, char **argv)
                 last_request = ros::Time::now();
             }
         }
-		if(des_waypoints == true){
+		// Update desired waypoints
-			pose.pose.position.x = wpoints.pose.position.x;
+        waypoint_cl.call(wpoint_srv);
-			pose.pose.position.y = wpoints.pose.position.y;
+        pose.pose.position.x = wpoint_srv.response.xd.x;
-		}
+        pose.pose.position.y = wpoint_srv.response.xd.y;
+        pose.pose.position.z = wpoint_srv.response.xd.z;
+		
 		
 		local_pos_pub.publish(pose);
 

src/path_follow.cpp

@@ -10,54 +10,37 @@
 #include <oscillation_ctrl/WaypointTrack.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 = Klausen
-geometry_msgs::Vector3 K_ang, J_ang; // Needed to populate EulerAngle msg
-oscillation_ctrl::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*/
+mavros_msgs::State current_state;
 void state_cb(const mavros_msgs::State::ConstPtr& msg){
     current_state = *msg;
 }
 
-geometry_msgs::Quaternion q_des;
+// Cb to determine attitude target
-// Cb to determine desired pose *Only needed for orientation 
+mavros_msgs::AttitudeTarget att_targ;
-void q_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
+void att_targ_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg){
-    desPose = *msg;
+    att_targ = *msg;
-    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;
@@ -75,12 +58,6 @@ void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
 	}
 }
 
-// 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");
@@ -91,31 +68,23 @@ int main(int argc, char **argv)
     ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
 		("mavros/state", 10, state_cb);
 
-    // Get quaternions from klausen control
+    // Get attitude target from klausen control
-    ros::Subscriber q_sub = nh.subscribe<geometry_msgs::PoseStamped>
+    ros::Subscriber att_target_sub = nh.subscribe<mavros_msgs::AttitudeTarget>
-		("command/quaternions",10,q_cb);
+		("command/att_target",10,att_targ_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);
-
     /********************** PUBLISHERS **********************/
     // 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>
+    ros::Publisher att_targ_pub = nh.advertise<mavros_msgs::AttitudeTarget>
 		("mavros/setpoint_raw/attitude",10);
 
-    // Publish attitude cmds in euler angles
-    ros::Publisher euler_pub = nh.advertise<oscillation_ctrl::EulerAngles>
-		("command/euler_angles",10);
-
     // Service Clients
     ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
 		("mavros/cmd/arming");
@@ -145,11 +114,6 @@ int main(int argc, char **argv)
     /*********** Initiate variables ************************/
 	//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
     ros::Rate rate_pub(75.0);
-    // 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
 
 	// Retrieve desired waypoints
 	oscillation_ctrl::WaypointTrack wpoint_srv;
@@ -191,69 +155,46 @@ int main(int argc, char **argv)
 
     while(ros::ok()){
 		if(gps_read == true){
-		// Now need to convert publish q to euler ang
+			if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){
-		tf2::Matrix3x3 m_K(q_cmd); tf2::Matrix3x3 m_J(q_Jae);
+				if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){
-		m_K.getRPY(rol_K, pch_K, yaw_K);
+				} else {
-		m_J.getRPY(rol_J, pch_J, yaw_J);
+					ROS_INFO("Could not enter offboard mode");
-
+				}
-		// Populate msg
+				//last_request = ros::Time::now();
-		K_ang.x = 180*rol_K/3.141;
+			} else {
-		K_ang.y = 180*pch_K/3.141;
+				if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){
-		K_ang.z = yaw_K;
+					if( arming_client.call(arm_cmd) && arm_cmd.response.success){
-
+						ROS_INFO("Vehicle armed");
-		J_ang.x = rol_J;
+					}
-		J_ang.y = pch_J;
+					last_request = ros::Time::now();
-		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){
-		    } 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(8.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;
 			}
+			
+			if(current_state.mode == "OFFBOARD" && current_state.armed){
+				ROS_INFO_ONCE("Spiri is taking off");
+				if(!takeoff){		
+					tkoff_req = ros::Time::now();
+					takeoff = true;
+				}
+			}
+			
+			// Determine which messages to send
+			if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff){
+				att_targ.header.stamp = ros::Time::now();
+				att_targ_pub.publish(att_targ);
+				att_cmds = true;
+				ROS_INFO("ATTITUDE CTRL");
+				ROS_INFO("Des Altitude: %.2f", alt_des);
+				ROS_INFO("Cur Altitude: %.2f", current_altitude);	
+			} else {
+				local_pos_pub.publish(buff_pose);
+				ROS_INFO("POSITION CTRL");
+				ROS_INFO("Des Altitude: %.2f", alt_des);
+				ROS_INFO("Cur Altitude: %.2f", current_altitude);	
+			}			
+			ros::spinOnce();
+			rate_pub.sleep();
 		}
-		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;
-			ROS_INFO("ATTITUDE CTRL");
-			ROS_INFO("Des Altitude: %.2f", alt_des);
-			ROS_INFO("Cur Altitude: %.2f", current_altitude);	
-		} else {
-			local_pos_pub.publish(buff_pose);
-			ROS_INFO("POSITION CTRL");
-			ROS_INFO("Des Altitude: %.2f", alt_des);
-			ROS_INFO("Cur Altitude: %.2f", current_altitude);	
-		}
-
-		// Publish euler angs
-		euler_pub.publish(eulAng);
-		
-		ros::spinOnce();
-		rate_pub.sleep();
-	}
     }
 
     return 0;

src/ref_signalGen.py

@@ -8,14 +8,10 @@ import numpy as np
 import time
 import math
 
-from pymavlink import mavutil
-from scipy import signal
 from scipy.integrate import odeint 
-
+from oscillation_ctrl.msg import  RefSignal, LoadAngles
-from oscillation_ctrl.msg import tethered_status, RefSignal, LoadAngles
+from oscillation_ctrl.srv import WaypointTrack
-from oscillation_ctrl.srv import WaypointTrack, WaypointTrackRequest
+from geometry_msgs.msg import Pose, PoseStamped, Point, TwistStamped 
-from controller_msgs.msg import FlatTarget
-from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point, TwistStamped 
 from sensor_msgs.msg import Imu
 from mavros_msgs.msg import State
 
@@ -32,23 +28,20 @@ class Main:
 			self.tstart		= rospy.get_time()	
 
 		self.dt 			= 1.0/rate
-		self.tmax			= self.dt
+		self.tmax			= self.dt # used to get desired pos, vel, and acc for next time step (tmax) 
 		self.n				= self.tmax/self.dt + 1
 		self.t 				= np.linspace(0, self.tmax, self.n) # Time array
 
 		# Message generation/ collection
-		self.state 			 = State()
+		self.vel_data	 = TwistStamped() # This is needed to get drone vel from gps
-		self.mode  			 = ''
+		self.imu_data	 = Imu() 				 # Needed for to get drone acc from IMU
-		self.vel_data		 = TwistStamped() # This is needed to get drone vel from gps
+		self.ref_sig	 = RefSignal()	 # Smooth Signal
-		self.imu_data		 = Imu() 				 # Needed for to get drone acc from IMU
-		self.ref_sig		 = FlatTarget()	 # Smooth Signal
 		self.load_angles = LoadAngles()
 
 		self.has_run	= 0		# Bool to keep track of first run instance
 		self.dr_pos 	= Pose()
 		self.dr_vel		= self.vel_data.twist.linear
 		self.dr_acc		= self.imu_data.linear_acceleration
-		self.dr_acc_p = self.imu_data.linear_acceleration
 
 		# Get tether length
 		self.param_exists = False
@@ -69,15 +62,14 @@ class Main:
 		#rospy.Subscriber('/status/twoBody_status', tethered_status, self.dronePos_cb)
 		rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb)
 		rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
-		rospy.Subscriber('/mavros/state', State, self.state_cb)
 
 # --------------------------------------------------------------------------------#		
 # 									PUBLISHERS
 # --------------------------------------------------------------------------------#
 		# Publish desired path to compute attitudes
-		self.pub_path = rospy.Publisher('/reference/path',FlatTarget,queue_size = 10)
+		self.pub_path = rospy.Publisher('/reference/path',RefSignal,queue_size = 10)
 		# Needed for geometric controller to compute thrust
-		self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10)
+		#self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10)
 
 		# timer(s), used to control method loop freq(s) as defined by the rate(s)
 		self.pub_tim  = rospy.Timer(rospy.Duration(1.0/rate), self.publisher)
@@ -108,8 +100,8 @@ class Main:
 		
 		# For saturation:
 		self.jmax = [3, 3, 1]
-		self.amax = [2, 2, 1]
+		self.amax = [5, 5, 1]
-		self.vmax = [3, 3, 1]
+		self.vmax = [10, 10, 1]		#[3, 3, 1]
 		self.max  =	[0, 3, 1.5, 3]  #[0, 3, 1.5, 3]
 		# create the array: [vmax; amax; jmax]
 		self.max_array = np.array([[self.vmax],[self.amax],[self.jmax]]).T
@@ -158,29 +150,8 @@ class Main:
 		self.ak 	= np.zeros(len(self.SF_delay_x[0]))
 		self.s_gain	= 0.0 # Gain found from sigmoid	
 # --------------------------------------------------------------------------------#		
-# 																CALLBACK FUNCTIONS
+# 								CALLBACK FUNCTIONS
 # --------------------------------------------------------------------------------#
-	def state_cb(self, data):
-		if self.state.armed != data.armed:
-			rospy.loginfo("armed state changed from {0} to {1}".format(
-				self.state.armed, data.armed))
-
-		if self.state.connected != data.connected:
-			rospy.loginfo("connected changed from {0} to {1}".format(
-				self.state.connected, data.connected))
-
-		if self.state.mode != data.mode:
-			rospy.loginfo("mode changed from {0} to {1}".format(
-				self.state.mode, data.mode))
-
-		if self.state.system_status != data.system_status:
-			rospy.loginfo("system_status changed from {0} to {1}".format(
-				mavutil.mavlink.enums['MAV_STATE'][
-					self.state.system_status].name, mavutil.mavlink.enums[
-						'MAV_STATE'][data.system_status].name))
-
-		self.state = data
-    # mavros publishes a disconnected state message on init
 
 	# Callback to get link names, states, and pload deflection angles
 	def loadAngles_cb(self,msg):
@@ -351,11 +322,11 @@ class Main:
 
 		for i in range(9): 
 			# Populate EPS_F buffer with desired change based on feedback
-			self.EPS_F[i] = self.EPS_I[i] #+ EPS_D[i] #+ EPS_D[i]
+			self.EPS_F[i] = self.EPS_I[i] + self.s_gain*EPS_D[i] #+ EPS_D[i]
 
 		# Populate msg with epsilon_final
 		self.ref_sig.header.stamp = rospy.Time.now()
-		self.ref_sig.type_mask  = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin
+		#self.ref_sig.type_mask  = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin
 		self.ref_sig.position.x = self.EPS_F[0]
 		self.ref_sig.position.y = self.EPS_F[1]
 		self.ref_sig.velocity.x = self.EPS_F[3]
@@ -364,9 +335,9 @@ class Main:
 		self.ref_sig.acceleration.y	= self.EPS_F[7]
 		
 		# Do not need to evaluate z
-		self.ref_sig.position.z = 5.0
+		self.ref_sig.position.z 	= self.xd.z
-		self.ref_sig.velocity.z	= 0
+		self.ref_sig.velocity.z		= 0.0
-		self.ref_sig.acceleration.z	= 0
+		self.ref_sig.acceleration.z	= 0.0
 		#self.ref_sig.position.z = self.EPS_F[2]
 		#self.ref_sig.velocity.z	= self.EPS_F[5]
 		#self.ref_sig.acceleration.z	= self.EPS_F[8]
@@ -411,7 +382,7 @@ class Main:
 
 		# Publish reference signal
 		self.pub_path.publish(self.ref_sig)
-		self.pub_ref.publish(self.ref_sig)
+		#self.pub_ref.publish(self.ref_sig) # for geometric controller
 
 		# Give user feedback on published message:
 		#self.screen_output()

src/thrust_controller.py

@@ -0,0 +1,173 @@
+#!/usr/bin/env python2.7
+
+### Cesar Rodriguez Sept 21
+### Used to control thrust of drone
+
+import rospy,  tf
+import numpy as np
+
+from geometry_msgs.msg import PoseStamped, TwistStamped
+from mavros_msgs.msg import AttitudeTarget
+
+class PID:
+	def __init__(self):
+		# rate(s)
+		rate = 35.0
+		self.dt = 1/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()
+		
+		# tuning gains
+		#self.Kp = 0.335
+		#self.Kd = 0.1
+		self.Kp = 2.7
+		self.Kd = 0.5
+
+		# drone var
+		self.drone_m = 1.437
+		self.max_a	 = 14.2
+		self.max_t   = self.drone_m*self.max_a
+
+		# message variables
+		self.pose = PoseStamped()
+		self.pose_buff = PoseStamped()
+		self.pose_buff.pose.position.z = 2.5
+
+		self.attitude = AttitudeTarget()
+		self.attitude.type_mask = 1|2|4 	# ignore body rate command
+		self.attitude.orientation.x = 0.0
+		self.attitude.orientation.y = 0.0
+		self.attitude.orientation.z = 0.0
+		self.attitude.orientation.w = 1.0
+
+		self.des_alt = 2.5
+		self.dr_vel = TwistStamped()
+		#self.cur_att = PoseStamped()
+		self.R = np.empty([3,3])		
+
+		# clients
+		
+
+		# subscribers
+		rospy.Subscriber('/mavros/local_position/pose',PoseStamped, self.pose_cb)
+		rospy.Subscriber('/mavros/local_position/velocity_local', TwistStamped, self.droneVel_cb)
+		
+		# publishers
+		self.pub_attitude = rospy.Publisher('/command/thrust', AttitudeTarget, queue_size=10)
+
+
+		# publishing rate
+		self.pub_time = rospy.Timer(rospy.Duration(1.0/rate), self.publisher)	
+
+	def pose_cb(self,msg):
+		self.cur_alt = msg.pose.position.z
+
+		self.pose = msg
+
+	# Callback for drone velocity
+	def droneVel_cb(self,msg):
+		try:
+			self.dr_vel = msg
+
+		except ValueError or TypeError:
+			pass
+
+
+	def quaternion_rotation_matrix(self):
+		"""
+		Covert a quaternion into a full three-dimensional rotation matrix.
+
+		Input
+		:param Q: A 4 element array representing the quaternion (q0,q1,q2,q3) 
+
+		Output
+		:return: A 3x3 element matrix representing the full 3D rotation matrix. 
+		         This rotation matrix converts a point in the local reference 
+		         frame to a point in the global reference frame.
+		"""
+		# Extract the values from Q
+		q0 = self.pose.pose.orientation.w
+		q1 = self.pose.pose.orientation.x
+		q2 = self.pose.pose.orientation.y
+		q3 = self.pose.pose.orientation.z
+		 
+		# First row of the rotation matrix
+		r00 = 2 * (q0 * q0 + q1 * q1) - 1
+		r01 = 2 * (q1 * q2 - q0 * q3)
+		r02 = 2 * (q1 * q3 + q0 * q2)
+		 
+		# Second row of the rotation matrix
+		r10 = 2 * (q1 * q2 + q0 * q3)
+		r11 = 2 * (q0 * q0 + q2 * q2) - 1
+		r12 = 2 * (q2 * q3 - q0 * q1)
+		 
+		# Third row of the rotation matrix
+		r20 = 2 * (q1 * q3 - q0 * q2)
+		r21 = 2 * (q2 * q3 + q0 * q1)
+		r22 = 2 * (q0 * q0 + q3 * q3) - 1
+		 
+		# 3x3 rotation matrix
+		rot_matrix = np.array([[r00, r01, r02],
+		                       [r10, r11, r12],
+		                       [r20, r21, r22]])
+		                        
+		return rot_matrix
+
+	def operation(self):
+		self.error = self.cur_alt - self.des_alt 		# error in z dir.
+		self.R = self.quaternion_rotation_matrix()	# determine Rotation Matrix 
+
+		# thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3)
+		# Taeyoung Lee, Melvin Leok, and N. Harris McClamroch
+		thrust = np.dot(9.81*self.drone_m -self.Kp*self.error -self.Kd*self.dr_vel.twist.linear.z,self.R.T[2])/self.max_t
+		self.attitude.thrust = thrust[2] # save thurst in z-dir
+
+		# Value needs to be between 0 - 1.0
+		if self.attitude.thrust >= 1.0:
+			self.attitude.thrust = 1.0
+		elif self.attitude.thrust <= 0.0:
+			self.attitude.thrust = 0.0
+
+
+	def publisher(self,pub_time):
+		self.operation() # determine thrust
+		self.attitude.header.stamp = rospy.Time.now()
+		self.pub_attitude.publish(self.attitude)
+
+	def user_feedback(self):
+		rospy.loginfo('Des: %.2f',self.des_alt)
+		rospy.loginfo('Error: %.2f',self.error)
+		rospy.loginfo('Throttle: %f\n',self.attitude.thrust)
+
+if __name__=="__main__":
+
+	# Initiate ROS node
+	rospy.init_node('thrust_PID')
+	try:
+		PID() 				# create class object
+		
+		rospy.spin() 	# loop until shutdown signal
+
+	except rospy.ROSInterruptException:
+	        pass
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+