Removed unused dependencies

.gitignore

@@ -1,4 +1,5 @@
 launch/debug.launch
+launch/mocap_*
 launch/cortex_bridge.launch
 src/MoCap_Localization_*.py
 src/Mocap_*.py
@@ -6,5 +7,6 @@ src/killswitch_client.py
 src/land_client.py
 msg/Marker.msg
 msg/Markers.msg
+launc
 *.rviz
 

CMakeLists.txt

@@ -54,6 +54,7 @@ add_message_files(
   tethered_status.msg
   RefSignal.msg
   EulerAngles.msg
+  LoadAngles.msg
 )
 
 ## Generate services in the 'srv' folder

launch/klausen_dampen.launch

@@ -31,11 +31,13 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 		pkg="oscillation_ctrl"
 		type="wpoint_tracker.py"
 		name="waypoints_server"
+		launch-prefix="xterm -e"
 	/>
 	<node
 		pkg="oscillation_ctrl"
 		type="ref_signalGen.py"
 		name="refSignal_node"
+		launch-prefix="xterm -e"
 	/>
 	<node
 		pkg="oscillation_ctrl"

src/LinkState.py

@@ -8,7 +8,7 @@ import rosservice
 import time
 import math
 from tf.transformations import *
-from offboard_ex.msg import tethered_status
+from oscillation_ctrl.msg import tethered_status, LoadAngles
 from geometry_msgs.msg import Pose
 from gazebo_msgs.srv import GetLinkState
 from std_msgs.msg import Bool
@@ -35,32 +35,31 @@ class Main:
 				self.wait	= rospy.get_param('sim/wait') # wait time
 				self.param_exists = True
 			elif rospy.get_time() - self.tstart >= 3.0:
+				self.wait = 0.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 
-
 		# Vehicle is spawned with yaw offset for convenience, need to deal with that
 		self.yaw_offset = 0.0
 
 		# variables for message gen
-		self.status 					= tethered_status()
+		self.status 		= tethered_status()
-		self.status.drone_id	= 'spiri_with_tether::spiri::base'
+		self.drone_id		= 'spiri_with_tether::spiri::base'
-		self.status.pload_id	= 'spiri_with_tether::mass::payload'
+		self.pload_id		= 'spiri_with_tether::mass::payload'
+		self.loadAngles	= LoadAngles()
+
+		# initialize variables
+		self.tetherL	= 0.0 # Tether length
+		self.has_run	= 0		# Boolean to keep track of first run instance
+		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 
 
 		# service(s)
 		self.service1 = '/gazebo/get_link_state'
@@ -69,7 +68,8 @@ class Main:
 		rospy.wait_for_service(self.service1,timeout=10)
 
 		# publisher(s)
-		self.publisher = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1)
+		self.twobody_pub = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1)
+		self.loadAng_pub = rospy.Publisher('/status/load_angles', LoadAngles, 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)
@@ -120,18 +120,18 @@ class Main:
 
 			# Establish links needed --> Spiri base and payload
 			# P = Position vector
-			drone_P = get_P(self.status.drone_id,reference)
+			drone_P = get_P(self.drone_id,reference)
 
 			# Get orientation of drone in euler angles to determine yaw offset
 			drone_Eul = self.euler_array(drone_P.link_state.pose.orientation)
 			
 			# Check if payload is part of simulation
 			if not drone_P.success:
-				self.status.drone_id	= 'spiri::base'
+				self.drone_id	= 'spiri::base'
-				drone_P = get_P(self.status.drone_id,reference) # i.e. no payload
+				drone_P = get_P(self.drone_id,reference) # i.e. no payload
 				self.pload = False
 
-			pload_P = get_P(self.status.pload_id,reference)
+			pload_P = get_P(self.pload_id,reference)
 
 			if not self.has_run == 1:
 				if self.pload == True:
@@ -184,30 +184,30 @@ class Main:
 				x_sep = pload_Px - drone_Px
 
 				if math.fabs(x_sep) >= self.tetherL or x_sep == 0:
-					self.theta = 0
+					self.loadAngles.theta = 0
 				else:
-					self.theta = math.asin(x_sep/self.tetherL)
+					self.loadAngles.theta = math.asin(x_sep/self.tetherL)
 
 				# Determine thetadot
-				self.thetadot = (self.theta - self.thetabuf)/self.dt
+				self.loadAngles.thetadot = (self.loadAngles.theta - self.thetabuf)/self.dt
-				self.thetadot = self.cutoff(self.thetadot,self.thetadot_max)
+				self.loadAngles.thetadot = self.cutoff(self.loadAngles.thetadot,self.thetadot_max)
-				self.thetabuf = self.theta
+				self.thetabuf = self.loadAngles.theta
 			
 				# Determine phi (roll)
 				y_sep = pload_Py - drone_Py
 
 				if math.fabs(y_sep) >= self.tetherL or y_sep == 0:
-					self.phi = 0
+					self.loadAngles.phi = 0
 				else:
-					self.phi = math.asin(y_sep/self.tetherL)
+					self.loadAngles.phi = math.asin(y_sep/self.tetherL)
 
 				# Determine phidot
-				self.phidot = (self.phi - self.phibuf)/self.dt
+				self.loadAngles.phidot = (self.loadAngles.phi - self.phibuf)/self.dt
-				self.phidot = self.cutoff(self.phidot,self.phidot_max)
+				self.loadAngles.phidot = self.cutoff(self.loadAngles.phidot,self.phidot_max)
-				self.phibuf = self.phi	# Update buffer
+				self.phibuf = self.loadAngles.phi	# Update buffer
 				
 			else: # Otherwise, vars = 0			
-				x_sep = self.phi = self.phidot = self.theta = self.thetadot = 0
+				x_sep = self.loadAngles.phi = self.loadAngles.phidot = self.loadAngles.theta = self.thetadot = 0
 
 			# Print and save results
 			print "\n"
@@ -216,21 +216,18 @@ class Main:
 			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 "phi:	        " + str(round(self.loadAngles.phi*180/3.14,3))
-			print "theta:          " + str(round(self.theta*180/3.14,3))
+			print "theta:          " + str(round(self.loadAngles.theta*180/3.14,3))
 
 			# Populate message
 			self.status.header.stamp = rospy.Time.now()
 			self.status.drone_pos = drone_P.link_state.pose
 			self.status.pload_pos = pload_P.link_state.pose
-			self.status.length		= self.tetherL
+			self.loadAngles.header.stamp = rospy.Time.now()
-			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)			
+			self.twobody_pub.publish(self.status)
+			self.loadAng_pub.publish(self.loadAngles)
 
 		except rospy.ServiceException as e:
 			rospy.loginfo("Get Link State call failed: {0}".format(e))

src/klausen_control.py

@@ -12,15 +12,15 @@ import numpy as np
 import time
 import math
 
-from tf.transformations  	import *
+from tf.transformations  	 import *
-from scipy.integrate 	 	 	import odeint 
+from scipy.integrate 	 	 	 import odeint 
-from offboard_ex.msg  	 	import tethered_status, RefSignal
+from oscillation_ctrl.msg  import tethered_status, RefSignal, LoadAngles
-from controller_msgs.msg 	import FlatTarget
+from controller_msgs.msg 	 import FlatTarget
-from geometry_msgs.msg 		import Point, Pose
+from geometry_msgs.msg 		 import Point, Pose
-from geometry_msgs.msg 		import TwistStamped, Vector3, Vector3Stamped, PoseStamped, Quaternion
+from geometry_msgs.msg 		 import TwistStamped, Vector3, Vector3Stamped, PoseStamped, Quaternion
-from gazebo_msgs.srv 	 		import GetLinkState
+from gazebo_msgs.srv 	 		 import GetLinkState
-from sensor_msgs.msg 	 		import Imu
+from sensor_msgs.msg 	 		 import Imu
-from pymavlink 		 				import mavutil
+from pymavlink 		 				 import mavutil
 
 class Main:
 
@@ -46,19 +46,14 @@ class Main:
 		self.path_pos		= np.zeros([3,1])
 		self.path_vel		=	np.zeros([3,1]) 
 		self.path_acc 	= np.zeros([3,1]) 
-		self.pl_pos 		= Pose()		
 		self.dr_pos 		= Pose()
 		self.quaternion = PoseStamped()
+		self.load_angles = LoadAngles()
 		self.EulerAng		= [0,0,0] # Will find the euler angles, and then convert to q
 
 		# Drone var
-		self.phi				= 0.0 #	Payload angle of deflection from x-axis
-		self.phidot			=	0.0
-		self.theta			= 0.0 # Payload angle of deflection from y-axis
-		self.thetadot		= 0.0
 		self.has_run		= 0		# Bool to keep track of first run instance
-		self.drone_id 	= ''
+
-		self.pload_id 	= ''
 		# Col1 = theta, theta dot; Col2 = phi, phidot for self.PHI
 		self.PHI 				=	np.array([[0,0],[0,0]]) 
 		self.dr_vel			= np.zeros([3,1])
@@ -105,7 +100,7 @@ class Main:
 # 																	SUBSCRIBERS
 # --------------------------------------------------------------------------------#
 		# Topic, msg type, and class callback method
-		rospy.Subscriber('/status/twoBody_status', tethered_status, self.linkState_cb)
+		rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb)
 		rospy.Subscriber('/reference/path', FlatTarget, self.refsig_cb)
 		rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb)
 		rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
@@ -125,21 +120,22 @@ class Main:
 # 																CALLBACK FUNCTIONS
 # --------------------------------------------------------------------------------#
 
-	# Callback to get link names, states, and pload deflection angles
+	# Callback pload deflection angles and vel
-	def linkState_cb(self,msg):
+	def loadAngles_cb(self,msg):
 		try:
-			self.drone_id = msg.drone_id
+			self.load_angles = msg
-			self.pload_id = msg.pload_id
-			self.dr_pos 	= msg.drone_pos
-			self.pl_pos 	= msg.pload_pos
-			#self.tetherL  = msg.length
-			self.phi			= msg.phi
-			self.phidot		=	msg.phidot
-			self.theta		= msg.theta
-			self.thetadot = msg.thetadot
 
 			# Populate self.PHI
-			self.PHI = np.array([[self.theta,self.thetadot],[self.phi,self.phidot]])
+			self.PHI = np.array([[self.load_angles.theta,self.load_angles.thetadot],[self.load_angles.phi,self.load_angles.phidot]])
+
+		except ValueError:
+			pass
+
+
+	# Callback drone pose
+	def dronePos_cb(self,msg): ### NEED to add mavros/local_pos.. sub
+		try:
+			self.dr_pos 	= msg.pose
 
 		except ValueError:
 			pass
@@ -195,14 +191,14 @@ class Main:
 	def control(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.theta],[self.phi]])
+		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]])
  
 		# State some variables for shorthand
 		L 			= self.tetherL
-		c_theta = math.cos(self.theta)
+		c_theta = math.cos(self.load_angles.theta)
-		c_phi 	= math.cos(self.phi)
+		c_phi 	= math.cos(self.load_angles.phi)
-		s_theta = math.sin(self.theta)
+		s_theta = math.sin(self.load_angles.theta)
-		s_phi		= math.sin(self.phi)
+		s_phi		= math.sin(self.load_angles.phi)
 
 		# Check for tether
 		if L <= 0.01:
@@ -224,15 +220,15 @@ class Main:
 
 				 [L*self.pl_m*c_theta, L*self.pl_m*s_phi*s_theta, -L*self.pl_m*c_phi*s_theta, 0, L**2*self.pl_m]]
 
-		C = [[0,0,0,0,-L*self.thetadot*self.pl_m*s_theta],
+		C = [[0,0,0,0,-L*self.load_angles.thetadot*self.pl_m*s_theta],
 
-				 [0,0,0,L*self.pl_m*(self.phidot*c_theta*s_phi + self.thetadot*c_phi*s_theta), L*self.pl_m*(self.phidot*c_phi*s_theta  + self.thetadot*c_theta*s_phi)],
+				 [0,0,0,L*self.pl_m*(self.load_angles.phidot*c_theta*s_phi + self.load_angles.thetadot*c_phi*s_theta), L*self.pl_m*(self.load_angles.phidot*c_phi*s_theta  + self.load_angles.thetadot*c_theta*s_phi)],
 
-				 [0,0,0,-L*self.pl_m*(self.phidot*c_phi*c_theta - self.thetadot*s_phi*s_theta),-L*self.pl_m*(self.thetadot*c_phi*c_theta - self.phidot*s_phi*s_theta)],
+				 [0,0,0,-L*self.pl_m*(self.load_angles.phidot*c_phi*c_theta - self.load_angles.thetadot*s_phi*s_theta),-L*self.pl_m*(self.load_angles.thetadot*c_phi*c_theta - self.load_angles.phidot*s_phi*s_theta)],
 
-				 [0,0,0,-0.5*(L**2)*self.pl_m*self.thetadot*math.sin(2*self.theta) + 0.5*0.01*self.thetadot*math.sin(2*self.theta), -0.5*(L**2)*self.pl_m*self.phidot*math.sin(2*self.theta)],
+				 [0,0,0,-0.5*(L**2)*self.pl_m*self.load_angles.thetadot*math.sin(2*self.load_angles.theta) + 0.5*0.01*self.load_angles.thetadot*math.sin(2*self.load_angles.theta), -0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta)],
 
-				 [0,0,0,0.5*(L**2)*self.pl_m*self.phidot*math.sin(2*self.theta),0]]
+				 [0,0,0,0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta),0]]
 
 		G = [[0],[0],[-9.81*self.tot_m],[L*9.81*self.pl_m*c_theta*s_phi],[L*9.81*self.pl_m*c_phi*s_theta]]
 

src/ref_signalGen.py

@@ -12,7 +12,7 @@ from pymavlink import mavutil
 from scipy import signal
 from scipy.integrate import odeint 
 
-from oscillation_ctrl.msg import tethered_status, RefSignal
+from oscillation_ctrl.msg import tethered_status, RefSignal, LoadAngles
 from oscillation_ctrl.srv import WaypointTrack, WaypointTrackRequest
 from controller_msgs.msg import FlatTarget
 from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point, TwistStamped 
@@ -39,22 +39,14 @@ class Main:
 		self.t 				= np.linspace(0, self.tmax, self.n) # Time array
 
 		# Message generation/ collection
-		self.state = State()
+		self.state 			 = State()
-		self.mode  = ''
+		self.mode  			 = ''
-		self.vel_data	= TwistStamped() # This is needed to get drone vel from gps
+		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.ref_sig	=	FlatTarget()	 # Smooth Signal
+		self.ref_sig		 = FlatTarget()	 # Smooth Signal
-		self.ref_sig.position.z = 5.0  # This does not need to be determined 
+		self.load_angles = LoadAngles()
 
-		self.phi			= 0.0 #	Payload angle of deflection from x-axis
-		self.phidot   = 0.0
-		self.theta		= 0.0 # Payload angle of deflection from y-axis
-		self.thetadot = 0.0
 		self.has_run	= 0		# Bool to keep track of first run instance
-		self.drone_id = ''
-		self.pload_id = ''
-
-		self.pl_pos 	= Pose()		
 		self.dr_pos 	= Pose()
 		self.dr_vel		= self.vel_data.twist.linear
 		self.dr_acc		= self.imu_data.linear_acceleration
@@ -74,7 +66,8 @@ class Main:
 # 																	SUBSCRIBERS																		#
 # --------------------------------------------------------------------------------#
 		# Topic, msg type, and class callback method
-		rospy.Subscriber('/status/twoBody_status', tethered_status, self.linkState_cb)
+		rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_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)
 		rospy.Subscriber('/mavros/state', State, self.state_cb)
@@ -198,18 +191,17 @@ class Main:
     # mavros publishes a disconnected state message on init
 
 	# Callback to get link names, states, and pload deflection angles
-	def linkState_cb(self,msg):
+	def loadAngles_cb(self,msg):
 		try:
-			self.drone_id = msg.drone_id
+			self.load_angles = msg							
-			self.pload_id = msg.pload_id
+		except ValueError:
-			self.dr_pos 	= msg.drone_pos
+			pass
-			self.pl_pos 	= msg.pload_pos
+
-			self.phi			= -msg.phi
+	# Callback drone pose
-			self.phidot   = -msg.phidot 
+	def dronePos_cb(self,msg): ### NEED to add mavros/local_pos.. sub
-			self.theta		= msg.theta
+		try:
-			self.thetadot = msg.thetadot
+			self.dr_pos 	= msg.pose
-#			self.tetherL	= msg.length
+
-							
 		except ValueError:
 			pass
 
@@ -298,13 +290,13 @@ class Main:
 		elif cmd == 2: # Feedback
 			if self.FB_idx < len(self.theta_fb):
 				# First, fill up the delay array
-				self.theta_fb[self.FB_idx] 		 = self.theta
+				self.theta_fb[self.FB_idx] 		 = self.load_angles.theta
-				self.theta_vel_fb[self.FB_idx] = self.thetadot
+				self.theta_vel_fb[self.FB_idx] = self.load_angles.thetadot
-				self.theta_acc_fb[self.FB_idx] = self.thetadot - self.theta_vel_fb[self.FB_idx-1]
+				self.theta_acc_fb[self.FB_idx] = self.load_angles.thetadot - self.theta_vel_fb[self.FB_idx-1]
 
-				self.phi_fb[self.FB_idx] 			 = self.phi
+				self.phi_fb[self.FB_idx] 			 = self.load_angles.phi
-				self.phi_vel_fb[self.FB_idx]   = self.phidot
+				self.phi_vel_fb[self.FB_idx]   = self.load_angles.phidot
-				self.phi_acc_fb[self.FB_idx]   = self.phidot - self.phi_vel_fb[self.FB_idx-1]
+				self.phi_acc_fb[self.FB_idx]   = self.load_angles.phidot - self.phi_vel_fb[self.FB_idx-1]
 
 			else:
 				# once array is filled, need to shift values w/ latest value at end
@@ -316,13 +308,13 @@ class Main:
 				self.phi_vel_fb[:] = np.roll(self.phi_vel_fb[:],-1)
 				self.phi_acc_fb[:] = np.roll(self.phi_acc_fb[:],-1)			
 		
-				self.theta_fb[len(self.theta_fb)-1] 		= self.theta # change final value
+				self.theta_fb[len(self.theta_fb)-1] 		= self.load_angles.theta # change final value
-				self.theta_vel_fb[len(self.theta_fb)-1] = self.thetadot
+				self.theta_vel_fb[len(self.theta_fb)-1] = self.load_angles.thetadot
-				self.theta_acc_fb[len(self.theta_fb)-1] = self.thetadot - self.theta_vel_fb[len(self.theta_fb)-1]
+				self.theta_acc_fb[len(self.theta_fb)-1] = self.load_angles.thetadot - self.theta_vel_fb[len(self.theta_fb)-1]
 
-				self.phi_fb[len(self.phi_fb)-1] 			  = self.phi
+				self.phi_fb[len(self.phi_fb)-1] 			  = self.load_angles.phi
-				self.phi_vel_fb[len(self.theta_fb)-1]   = self.phidot
+				self.phi_vel_fb[len(self.theta_fb)-1]   = self.load_angles.phidot
-				self.phi_acc_fb[len(self.theta_fb)-1]   = self.phidot - self.phi_vel_fb[len(self.theta_fb)-1]
+				self.phi_acc_fb[len(self.theta_fb)-1]   = self.load_angles.phidot - self.phi_vel_fb[len(self.theta_fb)-1]
 
 		else:
 			print('No delay')