diff --git a/src/mocap_runTest.py b/src/mocap_runTest.py deleted file mode 100755 index 77c87af..0000000 --- a/src/mocap_runTest.py +++ /dev/null @@ -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 \ No newline at end of file