#!/usr/bin/env python2.7 ### Cesar Rodriguez June 2021 ### Script to determine payload and drone state from Gazebo import rospy, tf import rosservice import time import math from tf.transformations import * from oscillation_ctrl.msg import TetheredStatus, LoadAngles, LoadStatus from gazebo_msgs.srv import GetLinkState from std_msgs.msg import Bool class Main: def __init__(self): # rate(s) rate = 60 # rate for the publisher method, specified in Hz -- 20 Hz self.dt = 1.0/rate rospy.sleep(5) # Sleep for 1 sec. Need to give time to Gazebo to ru # 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() # How long should we wait before before starting test self.param_exists = False while self.param_exists == False: if rospy.has_param('wait_time'): self.wait = rospy.get_param('sim/wait') # wait time self.param_exists = True elif rospy.get_time() - self.tstart >= 3.0: self.wait = 0.0 break # Will be set to true when test should start self.bool = False # variables for message gen self.status = TetheredStatus() self.drone_id = 'spiri_with_tether::spiri::base' self.pload_id = 'spiri_with_tether::mass::payload' self.loadAngles = LoadAngles() # initialize variables self.tetherL = 0.0 # Tether length self.has_run = 0 # Boolean to keep track of first run instance self.phibuf = 0.0 # Need buffers to determine their rates self.thetabuf = 0.0 # self.pload = True # Check if payload exists # service(s) self.service1 = '/gazebo/get_link_state' # wait for service to exist rospy.wait_for_service(self.service1,timeout=10) # 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) self.gui_timer = rospy.Timer(rospy.Duration(1/10.0), self.user_feedback) 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 # Get link states (drone and pload) and determine angle between them def link_state(self,pub_timer): try: # State which service we are querying get_P = rospy.ServiceProxy(self.service1,GetLinkState) # Set reference frame reference = '' # Establish links needed --> Spiri base and payload # P = Position vector drone_P = get_P(self.drone_id,reference) # Get orientation of drone in euler angles to determine yaw offset self.drone_Eul = self.euler_array(drone_P.link_state.pose.orientation) # Check if payload is part of simulation if not drone_P.success: self.drone_id = 'spiri::base' drone_P = get_P(self.drone_id,reference) # i.e. no payload self.pload = False pload_P = get_P(self.pload_id,reference) if not self.has_run == 1: if self.pload == True: # Determine yaw offset self.yaw_offset = self.drone_Eul[2] # 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 self.has_run = 1 # Need to detemine their location to get angle of deflection # Drone self.drone_Px = drone_P.link_state.pose.position.x self.drone_Py = drone_P.link_state.pose.position.y self.drone_Pz = drone_P.link_state.pose.position.z if self.pload == True: # If there is payload, determine the variables # 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 - self.drone_Px if math.fabs(x_sep) >= self.tetherL or x_sep == 0: self.loadAngles.theta = 0 else: self.loadAngles.theta = math.asin(x_sep/self.tetherL) # Determine thetadot self.loadAngles.thetadot = (self.loadAngles.theta - self.thetabuf)/self.dt self.thetabuf = self.loadAngles.theta # Determine phi (roll) y_sep = pload_Py - self.drone_Py if math.fabs(y_sep) >= self.tetherL or y_sep == 0: self.loadAngles.phi = 0 else: self.loadAngles.phi = -math.asin(y_sep/self.tetherL) # Determine phidot self.loadAngles.phidot = (self.loadAngles.phi - self.phibuf)/self.dt self.phibuf = self.loadAngles.phi # Update buffer else: # Otherwise, vars = 0 x_sep = self.loadAngles.phi = self.loadAngles.phidot = self.loadAngles.theta = self.thetadot = 0 # Populate message self.status.header.stamp = rospy.Time.now() self.status.drone_pos = drone_P.link_state.pose self.status.pload_pos = pload_P.link_state.pose self.loadAngles.header.stamp = rospy.Time.now() # Publish message self.twobody_pub.publish(self.status) self.loadAng_pub.publish(self.loadAngles) except rospy.ServiceException as e: rospy.loginfo("Get Link State call failed: {0}".format(e)) def user_feedback(self,gui_timer): print ("\n") rospy.loginfo("") print("Drone pos.x: " + str(round(self.drone_Px,2))) print("Drone pos.y: " + str(round(self.drone_Py,2))) print("Drone pos.z: " + str(round(self.drone_Pz,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.loadAngles.theta*180/3.14,2))) print("Phi: " + str(round(self.loadAngles.phi*180/3.14,2))) def path_follow(self,path_timer): now = rospy.get_time() if now - self.tstart < self.wait: self.bool = False else: self.bool = True self.pub_wd.publish(self.bool) if __name__=="__main__": # Initiate ROS node rospy.init_node('linkStates_node',anonymous=False) try: Main() # create class object rospy.spin() # loop until shutdown signal except rospy.ROSInterruptException: pass