212 lines
6.6 KiB
Python
Executable File
212 lines
6.6 KiB
Python
Executable File
#!/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
|