scorpio1
/
oscillation_ctrl
forked from cesar.alejandro/oscillation_ctrl
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Delete 'src/mocap_runTest.py'

This commit is contained in:
cesar.alejandro 2022-09-16 08:20:12 -07:00
parent 83ca1236a9
commit 13fd2671d1
1 changed files with 0 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
src/mocap_runTest.py
View File

@ -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