bug fixes

config/Ctrl_param.yaml

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

launch/klausen_dampen.launch

@@ -9,13 +9,18 @@ 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="test_type" default="step.py" />
+  <!--arg name="test_type" default="step.py" /-->
   <arg name="model" default="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>
+
 	<node
 		pkg="oscillation_ctrl"
 		type="LinkState.py"
@@ -48,7 +53,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 	
 	<node
 		pkg="oscillation_ctrl"
-		type="$(arg test_type)"
+		type="perform_test.py"
 		name="test_node"
 		launch-prefix="xterm -e"
 	/>

src/klausen_control.py

@@ -174,7 +174,7 @@ class Main:
 	# Check if vehicle has tether
 	def tether_check(self):
 		if self.tether == True:
-			rospy.loginfo('TETHER LENGTH:', self.tetherL)
+			rospy.loginfo_once('TETHER LENGTH: %.2f', self.tetherL)
 		else:
 			rospy.loginfo_once('NO TETHER DETECTED')
 

src/perform_test.py

@@ -0,0 +1,121 @@
+#!/usr/bin/env python2.7
+
+
+### Cesar Rodriguez June 2021
+### Script to generate set points which form a square with 2m side lengths
+import rospy,  tf
+import time
+from geometry_msgs.msg import Point
+from std_msgs.msg import Bool
+
+class Main:
+	# class method used to initialize variables once when the program starts
+	def __init__(self):
+
+		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()	
+
+		# variable(s)
+		self.Point	= Point() # Point which will actually be published
+		self.xd			= Point() # buffer point to determine current location
+
+		# Init x, y, & z coordinates
+		self.Point.x	= 0
+		self.Point.y	= 0
+		self.Point.z	= 3.5
+
+		self.xarray = [1,2,2,2,1,0,0]
+		self.yarray = [0,0,1,2,2,2,1]
+		self.i = 0
+		self.j = 0
+		self.buffer = 10
+		self.bool = False
+
+		self.param_exists = False # check in case param does not exist
+		while self.param_exists == False:
+			if rospy.has_param('status/test_type'):
+				self.test = rospy.get_param('status/test_type')
+				self.param_exists = True
+
+			elif rospy.get_time() - self.tstart >= 3.0 or rospy.get_param('status/test_type') == 'none':
+				self.test = 'none'
+				break
+
+		# subscriber(s), with specified topic, message type, and class callback method
+		rospy.Subscriber('/status/path_follow',Bool, self.wait_cb) 
+
+		# publisher(s), with specified topic, message type, and queue_size
+		self.publisher = rospy.Publisher('/reference/waypoints', Point, queue_size = 5) 	 	
+
+		# rate(s)
+		pub_rate = 1 # rate for the publisher method, specified in Hz
+
+		# timer(s), used to control method loop frequencies as defined by the rate(s)
+		self.pub_timer = rospy.Timer(rospy.Duration(1.0/pub_rate), self.pub)
+
+	def wait_cb(self,data):
+		self.bool = data
+
+	def waypoints_srv_cb(self):
+		rospy.wait_for_service('/status/waypoint_tracker')
+		try:
+			resp = self.get_xd(False,self.xd)
+			self.Point.z = resp.xd.z
+		except ValueError:
+			pass
+
+	def square(self):
+		# Check if i is too large. loop back to first point
+		if self.i >= len(self.xarray):
+			self.Point.x = 0
+			self.Point.y = 0
+
+		else:
+			self.Square_Point.x = self.xarray[self.i]
+			self.Square_Point.y = self.yarray[self.i]
+
+		self.Point = self.Square_Point	
+		self.j += 1
+		self.i = self.j // self.buffer
+
+	def step(self):
+		# Published desired msgs
+		self.Point.x = 5 # STEP (meters)
+
+	def user_feedback(self):
+		rospy.loginfo("Sending [Point x] %d [Point y] %d",
+									 self.Point.x, self.Point.y) 
+
+	def determine_test(self):
+		if self.test == 'step':
+			self.step()
+		elif self.test == 'square':
+			self.square()
+
+	# Publish messages
+	def pub(self,pub_timer):
+		if self.bool == False: 		
+			rospy.loginfo('Waiting...')
+		elif self.bool == False and self.test == 'none':
+			rospy.loginfo('NO TEST WILL BE PERFORMED')
+		else:
+			# Run either step or square test
+			self.determine_test()
+			# Publish message
+			self.publisher.publish(self.Point)
+			# Give user feedback
+			self.user_feedback()
+
+
+if __name__=="__main__":
+
+	# Initiate ROS node
+	rospy.init_node('test_node',anonymous=True)
+	try:
+		Main() # create class object
+		rospy.spin() # loop until shutdown signal
+
+	except rospy.ROSInterruptException:
+	        pass
+

src/ref_signalGen.py

@@ -24,7 +24,7 @@ class Main:
 	def __init__(self):
 
 		# rate(s)
-		rate = 30 # rate for the publisher method, specified in Hz -- 10 Hz
+		rate = 60 # rate for the publisher method, specified in Hz -- 10 Hz
 
 		# initialize variables
 		self.tstart		= rospy.get_time() # Keep track of the start time
@@ -107,6 +107,7 @@ class Main:
 			self.wn = self.w_tune
 		else:
 			self.wn				= math.sqrt(9.81/self.tetherL)
+
 		self.wd				= self.wn*math.sqrt(1 - self.epsilon**2)
 		self.k4				= 4*self.epsilon*self.w_tune
 		self.k3				= ((2 + 4*self.epsilon**2)*self.w_tune**2)/self.k4
@@ -365,7 +366,7 @@ 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] #+ EPS_D[i] #+ EPS_D[i]
 
 		# Populate msg with epsilon_final
 		self.ref_sig.header.stamp = rospy.Time.now()

src/square.py

@@ -26,6 +26,17 @@ class Main:
 		self.buffer = 10
 		self.bool = False
 
+		self.param_exists = False # check in case param does not exist
+		while self.param_exists == False:
+			if rospy.has_param('status/test_type'):
+				self.test = rospy.get_param('status/test_type')
+				self.param_exists = True
+
+			elif rospy.get_time() - self.tstart >= 3.0 || rospy.get_param('status/test_type') == 'none'
+				self.test = 'none'
+				break
+
+
 		# subscriber(s), with specified topic, message type, and class callback method
 		rospy.Subscriber('/status/path_follow',Bool, self.wait_cb)