diff --git a/config/Ctrl_param.yaml b/config/Ctrl_param.yaml
index 96d5581..c2ac39d 100644
--- a/config/Ctrl_param.yaml
+++ b/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}
diff --git a/launch/klausen_dampen.launch b/launch/klausen_dampen.launch
index 24bcb44..df8a76f 100644
--- a/launch/klausen_dampen.launch
+++ b/launch/klausen_dampen.launch
@@ -9,13 +9,18 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
-
+
+
+
+
+
+
diff --git a/src/klausen_control.py b/src/klausen_control.py
index 9a8eea2..65bbcb8 100755
--- a/src/klausen_control.py
+++ b/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')
diff --git a/src/perform_test.py b/src/perform_test.py
new file mode 100755
index 0000000..a509a02
--- /dev/null
+++ b/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
+
diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py
index 9fe107a..d7236fa 100755
--- a/src/ref_signalGen.py
+++ b/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,11 +366,11 @@ 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()
- self.ref_sig.type_mask = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin
+ 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]
diff --git a/src/square.py b/src/square.py
index 07e7ed3..88af1eb 100755
--- a/src/square.py
+++ b/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)