Weekly improvements

config/px4_config.yaml

@@ -208,7 +208,7 @@ landing_target:
 # mocap_pose_estimate
 mocap:
   # select mocap source
-  use_tf: false   # ~mocap/tf
+  use_tf: false  # ~mocap/tf
   use_pose: true  # ~mocap/pose
 
 # odom

config/px4_pluginlists.yaml

@@ -2,14 +2,18 @@ plugin_blacklist:
 # common
 - safety_area
 # extras
+- tdr_radio
 - image_pub
 - vibration
 - distance_sensor
 - rangefinder
 - wheel_odometry
 - mission
-- tdr_radio
 - fake_gps
+- setpoint_accel
+- landing_target
+- odometry
+- px4flow
 
 plugin_whitelist: []
 #- 'sys_*'

launch/cortex_bridge.launch

@@ -9,10 +9,12 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
   <arg name="fcu_protocol" default="v2.0" />
   <arg name="respawn_mavros" default="false" />
   <arg name="gazebo_gui" default="false" />
+  <!--arg name="fcu_url" default="udp://:14551@192.168.1.91:14556"/>
+  <arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" /-->
 
 	<node
 		pkg="oscillation_ctrl"
-		type="MoCap_Localization_noTether.py"
+		type="MocapBridge.py"
 		name="localize_node"
 		launch-prefix="xterm -e"
 	/>
@@ -21,7 +23,10 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 	<include file="$(find cortex_bridge)/launch/cortex_bridge.launch"/>
 
 	<!-- MAVROS launch  -->
-	<include file="$(find mavros)/launch/px4.launch"/>
+	<include file="$(find mavros)/launch/px4.launch">
 		<arg name="pluginlists_yaml" value="$(find oscillation_ctrl)/config/px4_pluginlists.yaml" />
 		<arg name="config_yaml" value="$(find oscillation_ctrl)/config/px4_config.yaml" />
+		<arg name="fcu_url" value="udp://:14549@192.168.1.91:14554" />
+		<arg name="gcs_url" value="udp-b://127.0.0.1:14555@14550" />
+	</include>
 </launch>

launch/klausen_dampen.launch

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

launch/px4.launch

@@ -0,0 +1,23 @@
+<launch>
+	<!-- vim: set ft=xml noet : -->
+	<!-- example launch script for PX4 based FCU's -->
+	<arg name="fcu_url" default="udp://:14551@192.168.1.91:14556"/>
+	<arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" />
+	<arg name="tgt_system" default="1" />
+	<arg name="tgt_component" default="1" />
+	<arg name="log_output" default="screen" />
+	<arg name="fcu_protocol" default="v2.0" />
+	<arg name="respawn_mavros" default="false" />
+
+	<include file="$(find mavros)/launch/node.launch">
+		<arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
+		<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
+		<arg name="fcu_url" value="$(arg fcu_url)" />
+		<arg name="gcs_url" value="$(arg gcs_url)" />
+		<arg name="tgt_system" value="$(arg tgt_system)" />
+		<arg name="tgt_component" value="$(arg tgt_component)" />
+		<arg name="log_output" value="$(arg log_output)" />
+		<arg name="fcu_protocol" value="$(arg fcu_protocol)" />
+		<arg name="respawn_mavros" default="$(arg respawn_mavros)" />
+	</include>
+</launch>

src/LinkState.py

@@ -18,7 +18,7 @@ class Main:
 	def __init__(self):
 
 		# rate(s)
-		rate = 40 # rate for the publisher method, specified in Hz -- 20 Hz
+		rate = 60 # rate for the publisher method, specified in Hz -- 20 Hz
 
 		self.dt		 = 1.0/rate
 
@@ -65,18 +65,9 @@ class Main:
 		# 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
 		rospy.wait_for_service(self.service1,timeout=10)
 
-#		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)
@@ -229,6 +220,7 @@ class Main:
 			print "theta:          " + str(round(self.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

src/MoCap_Localization_noTether.py

@@ -17,7 +17,7 @@ class Main:
 	def __init__(self):
 
 		# rate(s)
-		rate = 40 # rate for the publisher method, specified in Hz -- 20 Hz
+		rate = 100 # 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

src/MocapBridge.py

@@ -0,0 +1,195 @@
+#!/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 = 50 # 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])
+
+	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.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/klausen_control.py

@@ -36,6 +36,9 @@ class Main:
 		self.tmax			= self.dt
 		self.n				= self.tmax/self.dt + 1
 		self.t 				= np.linspace(0, self.tmax, self.n) # Time array
+		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()
 		
 		# Msgs types
 		self.vel_data		= TwistStamped() # This is needed to get drone vel from gps
@@ -69,7 +72,11 @@ class Main:
 			if rospy.has_param('sim/tether_length'):
 				self.tetherL	= rospy.get_param('sim/tether_length') # Tether length
 				self.param_exists = True
-		self.tether 		= True 			# Assume we have a tether
+				self.tether 			= True
+			elif rospy.get_time() - self.tstart >= 10.0:
+				self.tetherL = 0.0
+				rospy.loginfo('waiting for tether length')
+				break 
 
 		# Tuning gains
 		self.K1					= np.identity(3)
@@ -159,7 +166,7 @@ class Main:
 		try:
 			self.path_pos = np.array([[msg.position.x],[msg.position.y],[msg.position.z]])
 			self.path_vel = np.array([[msg.velocity.x],[msg.velocity.y],[msg.velocity.z]])
-			self.path_acc	=	np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z + 9.81]])
+			self.path_acc	=	np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z + 9.81]]) #TODO
 
 		except ValueError:
 			pass
@@ -167,10 +174,11 @@ class Main:
 	# Check if vehicle has tether
 	def tether_check(self):
 		if self.tether == True:
-			rospy.loginfo_once('USING TETHER MODEL')
+			rospy.loginfo('TETHER LENGTH:', self.tetherL)
 		else:
 			rospy.loginfo_once('NO TETHER DETECTED')
 
+
 # ---------------------------------ODE SOLVER-------------------------------------#
 	def statespace(self,y,t,Ka,Kb,Kc):
 		# Need the statespace array:

src/path_follow.cpp

@@ -30,7 +30,7 @@ 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
+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;
 
@@ -229,23 +229,19 @@ int main(int argc, char **argv)
 		attitude.thrust = thrust.thrust;
 		
 		// Determine which messages to send
-		if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff){
+		if(ros::Time::now() - tkoff_req > ros::Duration(22.0) && takeoff){
 			attitude.orientation = q_des;
 			attitude.header.stamp = ros::Time::now();
 			att_targ.publish(attitude);
 			att_cmds = true;
-		} else {
+			ROS_INFO("Attitude Ctrl");
-			local_pos_pub.publish(buff_pose);
+			ROS_INFO("Des Altitude: %.2f", alt_des);
-		}
+			ROS_INFO("Cur Altitude: %.2f", current_altitude);	
-
+		} else {
-		if(!att_cmds && ros::Time::now() - last_request > ros::Duration(2.0)){
+			local_pos_pub.publish(buff_pose);
-			ROS_INFO("Thro: %.2f", attitude.thrust);
+			ROS_INFO("Position Ctrl");
-		} 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

src/ref_signalGen.py

@@ -30,7 +30,6 @@ class Main:
 		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()	
-		rospy.loginfo(self.tstart)	
 
 		self.dt 			= 1.0/rate
 #		self.dt 			= 0.5
@@ -67,7 +66,7 @@ class Main:
 			if rospy.has_param('sim/tether_length'):
 				self.tetherL	= rospy.get_param('sim/tether_length') # Tether length
 				self.param_exists = True
-			elif rospy.get_time() - self.tstart >= 3.0:
+			elif rospy.get_time() - self.tstart >= 10.0:
 				self.tetherL	= 0.0
 				break
 		
@@ -371,15 +370,12 @@ class Main:
 		# 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.position.x = self.EPS_F[0]
 		self.ref_sig.position.y = self.EPS_F[1]
-		self.ref_sig.position.z = self.EPS_F[2] + 0.5
+		self.ref_sig.position.z = self.EPS_F[2] 
-
 		self.ref_sig.velocity.x = self.EPS_F[3]
 		self.ref_sig.velocity.y	= self.EPS_F[4]
 		self.ref_sig.velocity.z	= self.EPS_F[5]
-
 		self.ref_sig.acceleration.x	= self.EPS_F[6]
 		self.ref_sig.acceleration.y	= self.EPS_F[7]
 		self.ref_sig.acceleration.z	= self.EPS_F[8]
@@ -426,7 +422,6 @@ class Main:
 		self.pub_path.publish(self.ref_sig)
 		self.pub_ref.publish(self.ref_sig)
 
-
 		# Give user feedback on published message:
 		self.screen_output()
 

src/wpoint_tracker.py

@@ -46,7 +46,6 @@ class Main:
 		
 		# publisher(s)
 
-
 	def waypoint_relay(self,req):
 		#TODO Need to add check to make sure query is boolean
 		if req.query: