Added gitignore

.gitignore

@@ -0,0 +1,3 @@
+models
+citadel_hill_world.world
+

package.xml

@@ -25,15 +25,12 @@
   <exec_depend>geometry_msgs</exec_depend>
 
   <build_depend>std_msgs</build_depend>
-  <exec_depend>stds_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
 
   <build_depend>message_generation</build_depend>
   <build_depend>message_runtime</build_depend>
   <exec_depend>message_runtime</exec_depend>
 
-  <build_depend>px4</build_depend>
-  <exec_depend>px4</exec_depend>
-
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->

src/MoCap_Localization_noTether.py

@@ -0,0 +1,153 @@
+#!/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 oscillation_ctrl.msg import tethered_status
+from geometry_msgs.msg import PoseStamped
+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.drone_pose = PoseStamped()
+
+		# Max dot values to prevent 'blowup'
+		self.phidot_max   = 3.0
+		self.thetadot_max = 3.0 
+
+		# variables for message gen
+
+
+		# service(s)
+
+		# need service list to check if models have spawned
+
+
+		# wait for service to exist
+
+
+		# 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)
+
+		### Since there is no tether, we can publish directly to mavros
+		self.pose_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=1)
+		
+		self.pub_timer  = rospy.Timer(rospy.Duration(1.0/rate), self.publisher)
+
+		# subscriber(s)
+		rospy.Subscriber('/cortex/body_pose', PoseStamped, self.bodyPose_cb)
+	
+	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
+
+
+#  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)
+				
+	def bodyPose_cb(self,msg):
+		try:
+			self.drone_pose = msg
+							
+		except ValueError:
+			pass
+
+	def publisher(self,pub_timer):
+		self.pose_pub.publish(self.drone_pose)
+
+if __name__=="__main__":
+
+	# Initiate ROS node
+	rospy.init_node('MoCap_node',anonymous=False)
+	try:
+		Main() # create class object
+		rospy.spin() # loop until shutdown signal
+
+	except rospy.ROSInterruptException:
+	        pass
+

src/ref_signalGen.py

@@ -11,7 +11,7 @@ import math
 from scipy import signal
 from scipy.integrate import odeint 
 
-from offboard_ex.msg import tethered_status, RefSignal
+from oscillation_ctrl.msg import tethered_status, RefSignal
 
 from controller_msgs.msg import FlatTarget