From afc267bd1632e6c1d2a6fdff1c85047f0aa15006 Mon Sep 17 00:00:00 2001 From: "cesar.alejandro" Date: Fri, 16 Sep 2022 08:19:53 -0700 Subject: [PATCH] Delete 'src/MoCap_Localization_fake.py' --- src/MoCap_Localization_fake.py | 239 --------------------------------- 1 file changed, 239 deletions(-) delete mode 100755 src/MoCap_Localization_fake.py diff --git a/src/MoCap_Localization_fake.py b/src/MoCap_Localization_fake.py deleted file mode 100755 index 9a3134d..0000000 --- a/src/MoCap_Localization_fake.py +++ /dev/null @@ -1,239 +0,0 @@ -#!/usr/bin/env python2.7 - -### Cesar Rodriguez Mar 2022 -### Script to simulate mocap readings and see how PX4 behaves - -import rospy, tf -import rosservice -import time -import math -import random -from tf.transformations import * -from oscillation_ctrl.msg import TetheredStatus, LoadAngles -from geometry_msgs.msg import PoseStamped -from gazebo_msgs.srv import GetLinkState -from std_msgs.msg import Bool - -class Main: - - def __init__(self): - - # rate(s) - pub_rate = 50 # rate for the publisher method, specified in Hz -- 20 Hz - loc_rate = 60 # rate we want to localize vehicle -- 60 Hz - self.dt = 1.0/loc_rate - - self.user_fback = True - - rospy.sleep(5) # Sleep for 5 sec. Need to give time to Gazebo to run - - # Variables needed for testing start - 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() - - # initialize variables - self.tetherL = 0.0 # Tether length - self.has_run = False # Boolean to keep track of first run instance - self.phibuf = 0.0 # Need buffers to determine their rates - self.thetabuf = 0.0 # - self.pload = False # Check if payload exists - # Max dot values to prevent 'blowup' - self.angledot_max = 2.0 - self.drone_eul = [0.0,0.0,0.0] - - # variables for message gen - #self.buff_pose1 = PoseStamped() - self.drone_pose = PoseStamped() - self.pload_pose = PoseStamped() - self.load_angles = LoadAngles() - self.twobody_status = TetheredStatus() - self.drone_id = 'spiri_with_tether::spiri::base' - self.pload_id = 'spiri_with_tether::mass::payload' - - # service(s) - self.service1 = '/gazebo/get_link_state' - self.service2 = '/gazebo/set_link_properties' - - - # need service list to check if models have spawned - self.service_list = rosservice.get_service_list() - - # wait for service to exist - while self.service1 not in self.service_list: - print ("Waiting for models to spawn...") - self.service_list = rosservice.get_service_list() - if rospy.get_time() - self.tstart >= 10.0: - break - - # publisher(s) - self.twobody_pub = rospy.Publisher('/status/twoBody_status', TetheredStatus, queue_size=1) - self.loadAng_pub = rospy.Publisher('/status/load_angles', LoadAngles, queue_size=1) - #self.pub_wd = rospy.Publisher('/status/path_follow', Bool, queue_size=1) - - #self.pub_timer = rospy.Timer(rospy.Duration(1.0/rate), self.link_state) - #self.path_timer = rospy.Timer(rospy.Duration(40.0/rate), self.path_follow) - - ### Since there is no tether, we can publish directly to mavros - self.visionPose_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=1) - - self.loc_timer = rospy.Timer(rospy.Duration(1.0/loc_rate), self.mocap_localize) - self.pub_timer = rospy.Timer(rospy.Duration(1.0/pub_rate), self.publisher) - - # subscriber(s) - - def euler_array(self,orientation): - """ - Takes in pose msg object and outputs array of euler angs: - eul[0] = Roll - eul[1] = Pitch - eul[2] = Yaw - """ - eul = euler_from_quaternion([orientation.x, - orientation.y, - orientation.z, - orientation.w]) - return eul - - def mocap_localize(self,loc_timer): - """ - Uses Gazebo to simulate MoCap - """ - try: - # State which service we are querying - get_P = rospy.ServiceProxy(self.service1,GetLinkState) - - # Set reference frame - reference = '' # world ref frame - - # Establish links needed --> Spiri base and payload - # P = Position vector - drone_P = get_P(self.drone_id,reference) - - # Check if payload is part of simulation - if not drone_P.success: - self.drone_id = 'spiri_mocap::base' - drone_P = get_P(self.drone_id,reference) # i.e. no payload - - self.drone_P = drone_P - pload_P = get_P(self.pload_id,reference) - if pload_P.success: self.pload = True - if not self.has_run: - if self.pload == True: - # Get tether length based off initial displacement - self.tetherL = math.sqrt((drone_P.link_state.pose.position.x - - pload_P.link_state.pose.position.x)**2 + - (drone_P.link_state.pose.position.y - - pload_P.link_state.pose.position.y)**2 + - (drone_P.link_state.pose.position.z - - pload_P.link_state.pose.position.z)**2) - rospy.set_param('status/tether_length',self.tetherL) - - else: - self.tetherL = 0.0 - self.has_run = True - - # Need to detemine their location to get angle of deflection - # Drone - drone_Px = drone_P.link_state.pose.position.x - drone_Py = drone_P.link_state.pose.position.y - # Get drone orientation - - if self.pload == True: # If there is payload, determine the variables - self.twobody_status.pload = True - # Pload - pload_Px = pload_P.link_state.pose.position.x - pload_Py = pload_P.link_state.pose.position.y - - # Determine theta (pitch) - x_sep = pload_Px - drone_Px - - if math.fabs(x_sep) >= self.tetherL or x_sep == 0: - self.load_angles.theta = 0 - else: - self.load_angles.theta = math.asin(x_sep/self.tetherL) - - # Determine thetadot - # self.load_angles.thetadot = min(self.angledot_max,max((self.load_angles.theta - self.thetabuf)/self.dt,-self.angledot_max)) - self.load_angles.thetadot = (self.load_angles.theta - self.thetabuf)/self.dt - self.thetabuf = self.load_angles.theta - - # Determine phi (roll) - y_sep = pload_Py - drone_Py - - if math.fabs(y_sep) >= self.tetherL or y_sep == 0: - self.load_angles.phi = 0 - else: - self.load_angles.phi = -math.asin(y_sep/self.tetherL) - - # Determine phidot - # self.load_angles.phidot = min(self.angledot_max,max((self.load_angles.phi - self.phibuf)/self.dt,-self.angledot_max)) - self.load_angles.phidot = (self.load_angles.phi - self.phibuf)/self.dt - self.phibuf = self.load_angles.phi # Update buffer - - # save pload position - self.twobody_status.pload_pos = pload_P.link_state.pose - self.pload_pose.pose = self.twobody_status.pload_pos - else: # Otherwise, vars = 0 - x_sep = self.load_angles.phi = self.load_angles.phidot = self.load_angles.theta = self.load_angles.thetadot = 0 - - # Populate message - #self.status.drone_pos = drone_P.link_state.pose - self.drone_pose.pose = drone_P.link_state.pose - self.twobody_status.drone_pos = drone_P.link_state.pose - - except rospy.ServiceException as e: - rospy.loginfo("Get Link State call failed: {0}".format(e)) - - def add_noise(self): - # self.drone_pose.pose.position.x = self.drone_pose.pose.position.x - # self.drone_pose.pose.position.y = self.drone_pose.pose.position.y - # self.drone_pose.pose.position.z = self.drone_pose.pose.position.z - self.drone_pose.pose.orientation.x = self.drone_pose.pose.orientation.x + random.uniform(0,0.004) - self.drone_pose.pose.orientation.y = self.drone_pose.pose.orientation.y + random.uniform(0,0.004) - self.drone_pose.pose.orientation.z = self.drone_pose.pose.orientation.z + random.uniform(0,0.004) - self.drone_pose.pose.orientation.w = self.drone_pose.pose.orientation.w + random.uniform(0,0.004) - - def publisher(self,pub_timer): - # add noise to signal - self.add_noise() - # fill out necesssary fields - self.drone_pose.header.frame_id = "/map" - self.drone_pose.header.stamp = rospy.Time.now() - self.load_angles.header.stamp = rospy.Time.now() - # publish - self.visionPose_pub.publish(self.drone_pose) # publish pose to mavros - self.loadAng_pub.publish(self.load_angles) # publish load angles to controller - self.twobody_pub.publish(self.twobody_status) # actual pose. Redundant but nice to have - # get euler array for user feedback - self.drone_eul = self.euler_array(self.drone_pose.pose.orientation) - self.user_feedback() - - def user_feedback(self): - if self.user_fback: - print ("\n") - rospy.loginfo("") - print ("drone pos.x: " + str(round(self.drone_pose.pose.position.x,2))) - print ("drone pos.y: " + str(round(self.drone_pose.pose.position.y,2))) - print ("drone pos.z: " + str(round(self.drone_pose.pose.position.z,2))) - print ("Roll: " + str(round(self.drone_eul[0]*180/3.14,2))) - print ("Pitch: " + str(round(self.drone_eul[1]*180/3.14,2))) - print ("Yaw: " + str(round(self.drone_eul[2]*180/3.14,2))) - if self.pload: - print("Tether length: " + str(round(self.tetherL,2))) - print("Theta: " + str(round(self.load_angles.theta*180/3.14,2))) - print("Phi: " + str(round(self.load_angles.phi*180/3.14,2))) - else: - rospy.loginfo_once(self.tetherL) - -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 -