Delete 'src/mocap_runTest.py'

src/mocap_runTest.py

@@ -1,104 +0,0 @@
-#! /usr/bin/env python2.7
-# Cesar Rodriguez Aug 2022
-# Sets attitude mode as well as new waypoints
-
-import rospy
-import time
-
-from oscillation_ctrl.srv import WaypointTrack, WaypointTrackResponse
-from geometry_msgs.msg import Point
-
-class Main:
-    def __init__(self):
-
-        # initialize variables
-        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()	
-
-        # set up client
-        self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
-
-        # Set up desired waypoints for test
-        self.xd1 = Point()
-        self.xd1.x =  0.0
-        self.xd1.y = -0.5
-        self.xd1.z =  1.75
-
-        self.xd2 = Point()
-        self.xd2.x =  0.0
-        self.xd2.y =  2.0
-        self.xd2.z =  1.75
-        
-        # Determine if we want to run test with or without controller
-        ################# CHANGE THIS TO CHANGE TYPE Of TEST ###############################
-
-        self.change_mode = True # True = Change to oscillation damping mode after wait time
-        self.multiple_setpoins = True # True - will send multiple setpoints
-                                                                                        
-        #####################################################################################
-        if self.change_mode: self.loginfo_string = 'Attitude mode in...' 
-        else: self.loginfo_string = 'Staying in Position mode.'
-
-        self.get_wait_time() # get wait time
-        self.run_test()      # runs the test
-
-    def get_wait_time(self):
-        """ Determine desired wait time based on ros params"""
-        self.param_exists = False
-        while self.param_exists == False:
-            rospy.loginfo_once('Getting wait time')
-            if rospy.has_param('status/wait_time'):
-                self.wait_time	= rospy.get_param('status/wait_time') # Tether length
-                self.param_exists = True
-                rospy.loginfo('Wait time: %.2f',self.wait_time)
-            elif rospy.get_time() - self.tstart >= 30:
-                break
-
-    def run_test(self):
-        """ Waits desired amount before setting UAV to appropriate mode, and then sets up desired waypoints"""
-        run_test = False
-        use_ctrl = False
-        waypoint_sent = False
-        while not run_test:
-            time_left = self.wait_time - (rospy.get_time() - self.tstart)
-            if not rospy.get_time() - self.tstart > self.wait_time:
-                rospy.loginfo(self.loginfo_string + ' %.2f',time_left)
-            elif rospy.get_time() - self.tstart >= self.wait_time and not use_ctrl:
-                rospy.set_param('status/use_ctrl',self.change_mode)
-                self.t_param = rospy.get_time()
-                use_ctrl = True
-            if use_ctrl:
-                time_until_test1 = 25.0 - rospy.get_time() + self.t_param
-                time_until_test2 = 30.0 - rospy.get_time() + self.t_param
-                if time_until_test1 >= 0.0 and not waypoint_sent:
-                    rospy.loginfo('In %.2f\nSending waypoints: x = %.2f y = %.2f z = %.2f',time_until_test1,self.xd1.x,self.xd1.y,self.xd1.z)
-                elif not waypoint_sent and time_until_test1 < 0.0:
-                    waypoint_sent = True
-                    self.set_waypoint(self.xd1)
-
-                if time_until_test2 >= 0.0 and waypoint_sent:
-                    rospy.loginfo('In %.2f\nSending waypoints: x = %.2f y = %.2f z = %.2f',time_until_test2,self.xd2.x,self.xd2.y,self.xd2.z)
-                elif waypoint_sent and time_until_test2 < 0.0:
-                    self.set_waypoint(self.xd2)
-                    run_test = True
-                    break
-    
-    def set_waypoint(self,xd):
-        """ Set waypoints for oscillation controller """
-        rospy.wait_for_service('/status/waypoint_tracker')
-        try:
-            self.get_xd(True,xd)
-        except ValueError:
-            pass
-
-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