Updates

.gitignore

@@ -1,6 +1,9 @@
 launch/debug.launch
 launch/cortex_bridge.launch
 src/MoCap_Localization_*.py
-src/MocapBridge.py
+src/Mocap_*.py
 src/*_client.py
+msg/Marker.msg
+msg/Markers.msg
 *.rviz
+

launch/cortex_bridge.launch

@@ -14,7 +14,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 
 	<node
 		pkg="oscillation_ctrl"
-		type="MocapBridge.py"
+		type="Mocap_Bridge.py"
 		name="localize_node"
 		launch-prefix="xterm -e"
 	/>

msg/LoadAngles.msg

@@ -0,0 +1,6 @@
+# Reports position of drone, payload, and the roll angle between them
+std_msgs/Header header		# Header
+float32 phi			# Roll angle between drone and pload
+float32 phidot			# Roll rate
+float32 theta			# Pitch angle between drone and pload
+float32 thetadot		# Pitch rate

msg/tethered_status.msg

@@ -1,12 +1,6 @@
 # Reports position of drone, payload, and the roll angle between them
 std_msgs/Header header		# Header
-string drone_id			# Drone ID
+std_msgs/Bool 	bool		# Boolean, True = Payload
 geometry_msgs/Pose drone_pos	# Drone pose
-# sensor_msgs/Imu drone_acc	# Drone linear acceleration
-string pload_id			# Payload ID
 geometry_msgs/Pose pload_pos	# Payload pose
-float32 length			# Tether length
+
-float32 phi			# Roll angle between drone and pload
-float32 phidot			# Roll rate
-float32 theta			# Pitch angle between drone and pload
-float32 thetadot		# Pitch rate

src/MocapBridge.py

@@ -1,208 +0,0 @@
-#!/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 *
-import tf2_ros 
-from oscillation_ctrl.msg import tethered_status
-from geometry_msgs.msg import PoseStamped, Point, TransformStamped
-from std_msgs.msg import Bool
-from tf2_msgs.msg import TFMessage
-
-class Main:
-
-	def __init__(self):
-
-		# rate(s)
-		rate = 30 # rate for the publisher method, specified in Hz -- 20 Hz
-
-		# 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()
-		#self.buff_pose  = PoseStamped()
-		self.desired_frames = ['LabDrone','payload'] # models of markers of interest
-
-		# Create necessary variables based on models of interest
-		for idx,name in enumerate(self.desired_frames):
-			exec('self.{:s}_pose = PoseStamped()'.format(name)) # ex) self.LabDrone_pose = PoseStamped()
-			exec('self.{:s}_pose.header.frame_id = "/map"'.format(name))
-
-		self.eul = [0.0,0.0,0.0]
-		self.has_run = 0
-		self.model_idx = 0
-		# 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)
-		### 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('/tf', TFMessage, self.cortex_cb)
-
-	def cortex_cb(self,msg):
-
-		try:
-				# disect msg to get pose of each body
-				for i,data in enumerate(msg.transforms): 				 # get length (i) and msg (data)
-					if data.child_frame_id in self.desired_frames:
-						#self.noname(data.child_frame_id,1) # send that string to execute desired action
-						exec('self.{:s}_pose.pose.position = data.transform.translation'.format(data.child_frame_id))
-						exec('self.{:s}_pose.pose.orientation = data.transform.rotation'.format(data.child_frame_id))
-						if self.model_idx < i: # check to see how many bodies tracked by mocap do we care about
-							self.model_idx = i
-						
-		except ValueError:
-			pass
-
-	def noname(self,name,action):
-			"""
-			Takes in string (name) and runs desired action
-			"""
-			if action == 1:
-				exec('self.{:s}_pose.pose.position = data.transform.translation'.format(name))
-				exec('self.{:s}_pose.pose.orientation = data.transform.rotation'.format(name))
-			elif action == 2:
-				exec('self.pose_pub.publish(self.{:s}_pose)',)
-										
-
-	
-	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:
-			eul[0] = Roll
-			eul[1] = Pitch
-			eul[2] = Yaw
-		"""
-		self.eul = euler_from_quaternion([pose.orientation.x,
-															pose.orientation.y,
-															pose.orientation.z,
-															pose.orientation.w])
-
-#		self.q = quaternion_from_euler(self.eul[0],self.eul[1],self.eul[2])
-
-#		offset_yaw = math.pi/2 
-#		q_offset   = quaternion_from_euler(0,0,offset_yaw)
-
-#		self.q = quaternion_multiply(self.q,q_offset)
-
-#		self.eul = euler_from_quaternion([self.q[0],self.q[1],self.q[2],self.q[3]])
-#		self.LabDrone_pose.pose.orientation.x = self.q[0]
-#		self.LabDrone_pose.pose.orientation.y = self.q[1]
-#		self.LabDrone_pose.pose.orientation.z = self.q[2]
-
-	def FRD_Transform(self):
-		'''
-		Transforms mocap reading to proper coordinate frame
-		'''
-
-		self.LabDrone_pose.header.stamp = rospy.Time.now()
-		self.euler_array(self.LabDrone_pose.pose) # get euler angles of orientation for user
-
-  
-#  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 publisher(self,pub_timer):
-		self.FRD_Transform()
-		#self.fake_mocap() # used while debugging
-		#self.noname()
-		#if self.model_idx == 1:
-		self.pose_pub.publish(self.LabDrone_pose)
-		#elif self.model_idx == 2:
-		#	self.pose_pub.publish(self.LabDrone_pose)
-			#self.pose_pub.publish(payload stuff)
-		print "\n"
-		print "drone pos.x:    " + str(round(self.LabDrone_pose.pose.position.x,2))
-		print "drone pos.y:    " + str(round(self.LabDrone_pose.pose.position.y,2))
-		print "drone pos.z:    " + str(round(self.LabDrone_pose.pose.position.z,2))
-		print "Roll:           " + str(round(self.eul[0]*180/3.14,2))
-		print "Pitch:          " + str(round(self.eul[1]*180/3.14,2))
-		print "Yaw:            " + str(round(self.eul[2]*180/3.14,2))
-
-	def fake_mocap(self):
-		self.LabDrone_pose.header.stamp = rospy.Time.now()
-	#self.LabDrone_pose.header.stamp.secs = self.LabDrone_pose.header.stamp.secs - self.tstart
-		self.LabDrone_pose.pose.position.x = 0.0
-		self.LabDrone_pose.pose.position.y = 0.0
-		self.LabDrone_pose.pose.position.z = 0.5
-		self.LabDrone_pose.pose.orientation.x = 0.0
-		self.LabDrone_pose.pose.orientation.y = 0.0
-		self.LabDrone_pose.pose.orientation.z = 0.0
-		self.LabDrone_pose.pose.orientation.w = 1.0
-		self.euler_array(self.LabDrone_pose.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/offb_node.cpp

@@ -114,7 +114,7 @@ int main(int argc, char **argv)
     // Populate pose msg
     pose.pose.position.x = 0;
     pose.pose.position.y = 0;
-    pose.pose.position.z = 2.5;
+    pose.pose.position.z = 1.5;
     pose.pose.orientation.x = q_init.x;
     pose.pose.orientation.y = q_init.y;
     pose.pose.orientation.z = q_init.z;