From 5b224e599d28618ca6e6ce91c2430cecfd13009d Mon Sep 17 00:00:00 2001 From: cesar Date: Fri, 9 Sep 2022 18:03:40 -0300 Subject: [PATCH] Adding mocap files for GPS denied environments --- .gitignore | 3 - CMakeLists.txt | 6 +- config/Ctrl_param.yaml | 12 +- config/gazebo_config.yaml | 4 +- launch/mocap_oscillation_damp.launch | 93 +++++++++++ launch/mocap_sim.launch | 71 ++++++++ launch/mocap_takeoff_noCtrl.launch | 65 ++++++++ src/MoCap_Localization_fake.py | 239 +++++++++++++++++++++++++++ src/MoCap_Localization_noTether.py | 183 ++++++++++++++++++++ src/klausen_control.py | 120 +++++++++----- src/mocap_offb_node.cpp | 194 ++++++++++++++++++++++ src/mocap_path_follow.cpp | 215 ++++++++++++++++++++++++ src/mocap_runTest.py | 87 ++++++++++ src/path_follow.cpp | 60 +++---- src/ref_signalGen.py | 58 +++---- 15 files changed, 1295 insertions(+), 115 deletions(-) create mode 100644 launch/mocap_oscillation_damp.launch create mode 100644 launch/mocap_sim.launch create mode 100644 launch/mocap_takeoff_noCtrl.launch create mode 100755 src/MoCap_Localization_fake.py create mode 100755 src/MoCap_Localization_noTether.py create mode 100644 src/mocap_offb_node.cpp create mode 100644 src/mocap_path_follow.cpp create mode 100755 src/mocap_runTest.py diff --git a/.gitignore b/.gitignore index 065dea8..40f8a97 100644 --- a/.gitignore +++ b/.gitignore @@ -2,13 +2,10 @@ config/mocap_* launch/cortex_bridge.launch launch/debug.launch launch/klausen_dampen.launch -launch/mocap_* src/development_* src/killswitch_client.py src/land_client.py -src/MoCap_*.py src/Mocap_*.py -src/mocap_* src/segmented_tether.py src/segmented_tether_fast.py msg/Marker.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 1d72db3..e1717b5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -95,11 +95,11 @@ add_dependencies(pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPO #add_dependencies(mocap_offb_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS}) -#add_executable(mocap_pathFollow_node src/mocap_path_follow.cpp) +add_executable(mocap_pathFollow_node src/mocap_path_follow.cpp) -#target_link_libraries(mocap_pathFollow_node ${catkin_LIBRARIES}) +target_link_libraries(mocap_pathFollow_node ${catkin_LIBRARIES}) -#add_dependencies(mocap_pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(mocap_pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS}) ## Declare a C++ library # add_library(${PROJECT_NAME} diff --git a/config/Ctrl_param.yaml b/config/Ctrl_param.yaml index c2990d6..e50ad51 100644 --- a/config/Ctrl_param.yaml +++ b/config/Ctrl_param.yaml @@ -1,8 +1,14 @@ # Ros param when using Klausen Ctrl wait_time: 30 -#drone_mass: 0.5841 -drone_mass: 1.437 -pload_mass: 0.50 +#drone_mass: 0.614 # weight with new battery +drone_mass: 0.602 # weight with old battery +#drone_mass: 1.437 # spiri weight + +#pload_mass: 0.15 # Pload mass with 100g weight +pload_mass: 0.10 # Pload mass with 50g weight +#pload_mass: 0.05 # Pload mass with just basket +#pload_mass: 0.25 + use_ctrl: false diff --git a/config/gazebo_config.yaml b/config/gazebo_config.yaml index 4868e62..4148cfa 100644 --- a/config/gazebo_config.yaml +++ b/config/gazebo_config.yaml @@ -1,4 +1,6 @@ # Ros param when not using Klausen Ctrl -waypoints: {x: 0.0, y: 0.0, z: 1.5} +waypoints: {x: 0.0, y: -0.25, z: 1.5} square_x: [0.5,1,1,1,0.5,0,0] square_y: [0,0,0.5,1,1,1,0.5] +hover_throttle: 0.46 # with 500g +hover_throttle: 0.51 # with 250g??? diff --git a/launch/mocap_oscillation_damp.launch b/launch/mocap_oscillation_damp.launch new file mode 100644 index 0000000..4a86ca8 --- /dev/null +++ b/launch/mocap_oscillation_damp.launch @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/mocap_sim.launch b/launch/mocap_sim.launch new file mode 100644 index 0000000..9a353bb --- /dev/null +++ b/launch/mocap_sim.launch @@ -0,0 +1,71 @@ + + + + + + should be mocap but will use gazebo since it is still sim <--> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/mocap_takeoff_noCtrl.launch b/launch/mocap_takeoff_noCtrl.launch new file mode 100644 index 0000000..bc2574a --- /dev/null +++ b/launch/mocap_takeoff_noCtrl.launch @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/MoCap_Localization_fake.py b/src/MoCap_Localization_fake.py new file mode 100755 index 0000000..9a3134d --- /dev/null +++ b/src/MoCap_Localization_fake.py @@ -0,0 +1,239 @@ +#!/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 + diff --git a/src/MoCap_Localization_noTether.py b/src/MoCap_Localization_noTether.py new file mode 100755 index 0000000..1fcd148 --- /dev/null +++ b/src/MoCap_Localization_noTether.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python2.7 + +### Cesar Rodriguez Feb 2022 +### Script to determine payload and drone state using mocap + +import rospy, tf +import rosservice +import time +import math +from tf.transformations import * +from oscillation_ctrl.msg import tethered_status +from geometry_msgs.msg import PoseStamped, Point +from std_msgs.msg import Bool + +class Main: + + def __init__(self): + + # rate(s) + rate = 120 # rate for the publisher method, specified in Hz -- 20 Hz + + # 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() + + ### -*-*-*- Do not need this unless a test is being ran -*-*-*- ### + # How long should we wait before before starting test + #self.param_exists = False + #while self.param_exists == False: + # if rospy.has_param('sim/wait'): + # self.wait = rospy.get_param('sim/wait') # wait time + # self.param_exists = True + # elif rospy.get_time() - self.tstart >= 3.0: + # break + + # Will be set to true when test should start + #self.bool = False + ### -*-*-*- END -*-*-*- ### + + # initialize variables + self.drone_pose = PoseStamped() + self.buff_pose = PoseStamped() + + self.eul = [0.0,0.0,0.0] + + # Max dot values to prevent 'blowup' + self.phidot_max = 3.0 + self.thetadot_max = 3.0 + + # variables for message gen + + + # service(s) + + # need service list to check if models have spawned + + + # wait for service to exist + + + # publisher(s) + ### Since there is no tether, we can publish directly to mavros + self.pose_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=1) + + self.pub_timer = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) + + # subscriber(s) + rospy.Subscriber('/cortex/body_pose', PoseStamped, self.bodyPose_cb) + + def cutoff(self,value,ceiling): + """ + Takes in value and returns ceiling + if value > ceiling. Otherwise, it returns + value back + """ + # initilize sign + sign = 1 + + # check if value is negative + if value < 0.0: + sign = -1 + # Cutoff value at ceiling + if (value > ceiling or value < -ceiling): + output = sign*ceiling + else: + output = value + return output + + def euler_array(self): + """ + Takes in pose msg object and outputs array of euler angs: + eul[0] = Roll + eul[1] = Pitch + eul[2] = Yaw + """ + self.eul = euler_from_quaternion([self.drone_pose.pose.orientation.x, + self.drone_pose.pose.orientation.y, + self.drone_pose.pose.orientation.z, + self.drone_pose.pose.orientation.w]) + + self.q = quaternion_from_euler(self.eul[0],self.eul[1],self.eul[2]) + + offset_yaw = math.pi/2 + q_offset = quaternion_from_euler(0,0,-offset_yaw) + + self.q = quaternion_multiply(self.q,q_offset) + + self.eul = euler_from_quaternion([self.q[0],self.q[1],self.q[2],self.q[3]]) + self.drone_pose.pose.orientation.x = self.q[0] + self.drone_pose.pose.orientation.y = self.q[1] + self.drone_pose.pose.orientation.z = self.q[2] + self.drone_pose.pose.orientation.w = self.q[3] + + def FRD_Transform(self): + ''' + Transforms mocap reading to proper coordinate frame + ''' + +# self.drone_pose = self.buff_pose + self.drone_pose.header.frame_id = "/map" + +# self.drone_pose.pose.position.x = 0 +# self.drone_pose.pose.position.y = 0 +# self.drone_pose.pose.position.z = 0.5 + + #Keep the w same and change x, y, and z as above. +# self.drone_pose.pose.orientation.x = 0 +# self.drone_pose.pose.orientation.y = 0 +# self.drone_pose.pose.orientation.z = 0 +# self.drone_pose.pose.orientation.w = 1 + + self.euler_array() # get euler angles of orientation for user + +# self.drone_pose.pose.position.x = self.buff_pose.pose.position.y +# self.drone_pose.pose.position.y = self.buff_pose.pose.position.x +# self.drone_pose.pose.position.z = -self.buff_pose.pose.position.z + +# Keep the w same and change x, y, and z as above. +# self.drone_pose.pose.orientation.x = self.buff_pose.pose.orientation.y +# self.drone_pose.pose.orientation.y = self.buff_pose.pose.orientation.x +# self.drone_pose.pose.orientation.z = -self.buff_pose.pose.orientation.z +# self.drone_pose.pose.orientation.w = self.buff_pose.pose.orientation.w + +# 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) + + def bodyPose_cb(self,msg): + try: + self.drone_pose = msg + + except ValueError: + pass + + def publisher(self,pub_timer): + self.FRD_Transform() + self.pose_pub.publish(self.drone_pose) + 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.eul[0]*180/3.14,2)) + print "Pitch: " + str(round(self.eul[1]*180/3.14,2)) + print "Yaw: " + str(round(self.eul[2]*180/3.14,2)) + + +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 + diff --git a/src/klausen_control.py b/src/klausen_control.py index 0868484..6831ea8 100755 --- a/src/klausen_control.py +++ b/src/klausen_control.py @@ -32,7 +32,7 @@ class Main: def __init__(self): # rate(s) - rate = 25 # rate for the publisher method, specified in Hz -- 50 Hz + rate = 25 # rate for the publisher method, specified in Hz -- 50 Hz #25 # initialize variables @@ -55,6 +55,7 @@ class Main: self.att_targ = AttitudeTarget() # used to send quaternion attitude commands self.load_angles = LoadAngles() self.EulerAng = [0,0,0] # Will find the euler angles, and then convert to q + self.EulerPose = [0,0,0] # Service var self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack) @@ -74,6 +75,8 @@ class Main: self.param_exists = False self.tetherL = self.get_tether() self.tether = True if self.tetherL > 0.01 else False + # Check if tether was correctly detected + self.tether_check() # Retrieve drone and payload masses from config file [self.drone_m, self.pl_m] = self.get_masses() @@ -93,9 +96,11 @@ class Main: self.z2 = np.zeros([5,1]) # [vx;vy;vz;thetadot;phidot] - alpha # Tuning gains - self.K1 = np.identity(3) - self.K2 = np.identity(5) - self.tune = 0.1 # Tuning parameter + self.K1 = np.identity(3)#*0.1 + # self.K1 = np.array([[2,-1,0],[-1,2,-1],[0,-1,2]]) + self.tune_array = np.array([1,1,1,0.1,0.1]).reshape(5,1) + self.K2 = np.identity(5)#*self.tune_array + self.tune = 0.1 #1 # Tuning parameter self.dist = np.array([0,0,0,0.1,0.1]) # Wind disturbance # np.array([0,0,0,0.01,0.01]) # Gain terms self.Kp = np.identity(3) + np.dot(self.K2[:3,:3],self.K1) + self.tune*np.identity(3) @@ -106,16 +111,10 @@ class Main: # gains for thrust PD Controller #self.Kp = 3.0 #self.Kd = 3 - self.Kp_thrust = 1.5 - self.Kd_thrust = 1 - self.max_a = 14.2 #TODO - self.max_t = self.tot_m*self.max_a - self.R = np.empty([3,3]) # rotation matrix + self.Kp_thrust = 1.5 #3.0 #1.5 + self.Kd_thrust = 1.0 #3.0 # 1.0 + self.R = np.empty([3,3]) # rotation matrix self.e3 = np.array([[0],[0],[1]]) - self.gravity = np.array([[0],[0],[9.81]]) - - self.thrust_offset = 0.0 #1.0 # There was found to be a constant offset 0.7 - # Get scaling thrust factor, kf self.kf = self.get_kf() @@ -150,9 +149,11 @@ class Main: while param_exists == False: if rospy.has_param('status/tether_length'): tether_length = rospy.get_param('status/tether_length') # Tether length + rospy.loginfo('TETHER LENGTH IN CONFG FILE') param_exists = True - elif rospy.get_time() - self.tstart >= 5.0: + elif rospy.get_time() - self.tstart >= 10.0: tether_length = 0.0 + rospy.loginfo('TETHER LENGTH NOT FOUND IN PARAMS') break return tether_length @@ -164,7 +165,7 @@ class Main: if rospy.has_param('status/drone_mass'): drone_m = rospy.get_param('status/drone_mass') # wait time param_exists = True - elif rospy.get_time() - self.tstart >= 3.0: + elif rospy.get_time() - self.tstart >= 5.0: drone_m = 0.5841 rospy.loginfo('DRONE MASS NOT FOUND IN CONFIG FILE') break @@ -173,8 +174,9 @@ class Main: while param_exists == False: if rospy.has_param('status/pload_mass'): pl_m = rospy.get_param('status/pload_mass') # wait time + rospy.loginfo('PLOAD MASS FOUND') param_exists = True - elif rospy.get_time() - self.tstart >= 3.0: + elif rospy.get_time() - self.tstart >= 5.0: pl_m = 0.0 rospy.loginfo('PLOAD MASS NOT FOUND IN CONFIG FILE') break @@ -201,7 +203,7 @@ class Main: try: self.load_angles = msg # Populate self.PHI - self.PHI = np.array([[self.load_angles.theta,self.load_angles.thetadot],[self.load_angles.phi,self.load_angles.phidot]]) + self.PHI = np.array([[self.load_angles.theta,self.load_angles.phi],[self.load_angles.thetadot,self.load_angles.phidot]]) except ValueError: pass @@ -209,6 +211,7 @@ class Main: def dronePos_cb(self,msg): try: self.dr_pos = msg.pose + self.EulerPose = self.convert2eul(self.dr_pos.orientation) # self.dr_pos = msg.drone_pos except ValueError: pass @@ -236,7 +239,6 @@ class Main: self.path_pos = np.array([[msg.position.x],[msg.position.y],[msg.position.z]]) self.path_vel = np.array([[msg.velocity.x],[msg.velocity.y],[msg.velocity.z]]) self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z]]) #TODO - # self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z + 9.81]]) #TODO except ValueError: pass @@ -311,6 +313,37 @@ class Main: return rot_matrix + def convert2eul(self,quaternion_orientation): + """ + Convers quaternion in pose message into euler angles + + Input + :param Q: orientatiom pose message + + Output + :return: Array of euler angles + """ + x = quaternion_orientation.x + y = quaternion_orientation.y + z = quaternion_orientation.z + w = quaternion_orientation.w + + t0 = +2.0 * (w * x + y * z) + t1 = +1.0 - 2.0 * (x * x + y * y) + roll = math.atan2(t0, t1) + + t2 = +2.0 * (w * y - z * x) + t2 = +1.0 if t2 > +1.0 else t2 + t2 = -1.0 if t2 < -1.0 else t2 + pitch = math.asin(t2) + + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (y * y + z * z) + yaw = math.atan2(t3, t4) + + euler = [roll,pitch,yaw] + return euler + def determine_throttle(self): # thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3) # Taeyoung Lee, Melvin Leok, and N. Harris McClamroch @@ -330,7 +363,11 @@ class Main: # thrust = thrust_vector[0]*self.R_e3[0] + thrust_vector[1]*self.R_e3[1] + thrust_vector[2]*self.R_e3[2] ### OR: thrust_vector = (9.81*self.tot_m + self.Kp_thrust*self.error[2] + self.Kd_thrust*self.error_vel[2] - self.tot_m*self.path_acc[2])*self.kf - thrust = thrust_vector/(math.cos(self.EulerAng[0]*self.EulerAng[1])) ##### + # thrust = thrust_vector/(math.cos(self.EulerAng[0]*self.EulerAng[1])) ##### + thrust = thrust_vector/(math.cos(self.EulerPose[0])*math.cos(self.EulerPose[1])) + + # if given Fd...? + # thrust = self.kf*Fd/(math.cos(self.EulerPose[0])*math.cos(self.EulerPose[1])) # Value needs to be between 0 - 1.0 self.att_targ.thrust = max(0.0,min(thrust,1.0)) @@ -349,10 +386,7 @@ class Main: s_theta = math.sin(self.load_angles.theta) s_phi = math.sin(self.load_angles.phi) - # Check if tether was correctly detected - self.tether_check() - - # Control matrices - this may be better in _init_ + # Control matrices M = [[self.tot_m, 0, 0, 0, L*self.pl_m*c_theta], [0, self.tot_m, 0, -L*self.pl_m*c_phi*c_theta, L*self.pl_m*s_phi*s_theta], [0, 0, self.tot_m, -L*self.pl_m*c_theta*s_phi, -L*self.pl_m*c_phi*s_theta], @@ -386,6 +420,7 @@ class Main: M_c = M[:3,3:5] # M_1:3,4:5 - rows 1 to 3 and columns 4 to 5 C_c = C[:3,3:5] + # Constants from Eq. 49 Ka = -(D_a + C_a + self.K2[3:5,3:5]) Kb = -G_a + np.dot(self.K2[3:5,3:5],self.PHI[:,1]) - np.dot(M_b,self.path_acc - np.dot(self.K1,self.dr_vel - self.path_vel)) @@ -398,7 +433,7 @@ class Main: self.a45_buff = odeint(self.statespace,self.a45_0,self.t,args=(Ka,Kb,M_aI)) # Update a45_0 to new a45. Need to transpose to column vector - self.a45_0 = self.a45_buff[1,:] + self.a45_0 = self.a45_buff[-1,:] self.a45 = np.array([[self.a45_0[0]],[self.a45_0[1]]]) # Get alphadot_4:5 @@ -419,38 +454,37 @@ class Main: self.z2 = g - self.alpha B = np.dot(C_c,self.a45) + np.dot(M_c,self.a45dot) + + + dr_orientation = [self.dr_pos.orientation.x, self.dr_pos.orientation.y, self.dr_pos.orientation.z, self.dr_pos.orientation.w] + dr_orientation_inv = quaternion_inverse(dr_orientation) + # fix_force = self.path_acc + # fix_force = fix_force[:3].reshape(3,1) + # fix_force[0] = self.path_acc[1] + # fix_force[1] = self.path_acc[0] # Desired body-oriented forces # shouldnt it be tot_m*path_acc? Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(self.Kd,z1_dot) - np.dot(self.Kp,self.z1) - np.dot(self.Ki,0.5*self.dt*(self.z1 - self.z1_p)) - # Fd = B + G[:3] + self.tot_m*self.path_acc - np.dot(self.Kd,z1_dot) - np.dot(self.Kp,self.z1) - np.dot(self.Ki,0.5*self.dt*(self.z1 - self.z1_p)) + # Fd = B + G[:3] + self.tot_m*fix_force - np.dot(self.Kd,z1_dot) - np.dot(self.Kp,self.z1) - np.dot(self.Ki,0.5*self.dt*(self.z1 - self.z1_p)) # Update self.z1_p for "integration" self.z1_p = self.z1 # Covert Fd into drone frame - dr_orientation = [self.dr_pos.orientation.x, self.dr_pos.orientation.y, self.dr_pos.orientation.z, self.dr_pos.orientation.w] - dr_orientation_inv = quaternion_inverse(dr_orientation) + Fd_tf = Fd + Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],Fd[2],0.0],dr_orientation_inv)) # Real part of Fd needs = 0 - # Fd_tf = [Fd[0],Fd[1],Fd[2]] + # Convert forces to attiude *EulerAng[2] = yaw = 0 self.EulerAng[1] = math.atan(Fd_tf[0]/(self.drone_m*9.81)) # Pitch self.EulerAng[0] = math.atan(-Fd_tf[1]*math.cos(self.EulerAng[1])/(self.drone_m*9.81)) # Roll # rospy.loginfo('Fd before transform: %.2f, %.2f, %.2f' % Fd[0],Fd[1],Fd[2]) - # rospy.loginfo('Fd before transform: %.2f, %.2f, %.2f', Fd[0], Fd[1], Fd[2]) - # rospy.loginfo('Fd after transform: %.2f, %.2f, %.2f', Fd_tf[0],Fd_tf[1],Fd_tf[2]) - - # Convert Euler angles to quaternions - # for i,val in enumerate(self.EulerAng): - # if val*-1 < 0.0: - # sign = 1 - # else: - # sign = -1 - # if abs(val) >=0.44: - # self.EulerAng[i] = 0.44*sign q = quaternion_from_euler(self.EulerAng[0],self.EulerAng[1],self.EulerAng[2]) + self.user_fback(Fd,Fd_tf) + # Populate msg variable # Attitude control self.att_targ.header.stamp = rospy.Time.now() @@ -461,15 +495,17 @@ class Main: self.att_targ.orientation.z = q[2] self.att_targ.orientation.w = q[3] - def user_fback(self): + def user_fback(self,F_noTransform, F_Transform): + print('\n') rospy.loginfo('thrust: %.6f' % self.att_targ.thrust) - rospy.loginfo('\nroll: %.2f\npitch: %.2f',self.EulerAng[0]*180/3.14,self.EulerAng[1]*180/3.14) + rospy.loginfo('roll: %.2f pitch: %.2f',self.EulerAng[0]*180/3.14,self.EulerAng[1]*180/3.14) + rospy.loginfo('Fd before transform: %.2f, %.2f, %.2f', F_noTransform[0], F_noTransform[1], F_noTransform[2]) + rospy.loginfo('Fd after transform: %.2f, %.2f, %.2f', F_Transform[0],F_Transform[1],F_Transform[2]) def publisher(self,pub_time): self.determine_q() self.determine_throttle() self.pub_att_targ.publish(self.att_targ) - self.user_fback() # --------------------------------------------------------------------------------# # ALGORITHM ENDS diff --git a/src/mocap_offb_node.cpp b/src/mocap_offb_node.cpp new file mode 100644 index 0000000..ad53acc --- /dev/null +++ b/src/mocap_offb_node.cpp @@ -0,0 +1,194 @@ +/** + * @file offb_node.cpp + * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight + * Stack and tested in Gazebo SITL + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +/********* CALLBACK FUNCTIONS **********************/ +// Initiate variables +mavros_msgs::State current_state; +geometry_msgs::PoseStamped desPose; + +// Callback function which will save the current state of the autopilot. +// Allows to check connection, arming, and Offboard tags*/ +void state_cb(const mavros_msgs::State::ConstPtr& msg){ + current_state = *msg; +} + +// Cb to recieve pose information +// Initiate variables +geometry_msgs::PoseStamped pose; +geometry_msgs::Quaternion q_init; +geometry_msgs::PoseStamped mavPose; +bool pose_read = false; +double current_altitude; + +void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ + mavPose = *msg; + current_altitude = mavPose.pose.position.z; + while(pose_read == false){ + q_init = mavPose.pose.orientation; + if(q_init.x == 0.0 && q_init.w == 0.0){ + ROS_INFO("Waiting..."); + } else { + mavPose.pose.orientation.x = q_init.x; + mavPose.pose.orientation.y = q_init.y; + mavPose.pose.orientation.z = q_init.z; + mavPose.pose.orientation.w = q_init.w; + pose_read = true; + } + } + +} + +std_msgs::Bool connection_status; +// bool connection_status +// Determine if we are still receiving info from mocap and land if not +void connection_cb(const std_msgs::Bool::ConstPtr& msg){ + connection_status = *msg; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "offb_node"); + ros::NodeHandle nh; + + /********************** SUBSCRIBERS **********************/ + // Get current state + ros::Subscriber state_sub = nh.subscribe + ("mavros/state", 10, state_cb); + + // Pose subscriber + ros::Subscriber mavPose_sub = nh.subscribe + ("mavros/local_position/pose",10,mavPose_cb); + + ros::Subscriber connection_sub = nh.subscribe + ("/status/comms",10,connection_cb); + + // Waypoint Subscriber + /* + ros::Subscriber waypoint_sub = nh.subscribe + ("/reference/waypoints",10,waypoints_cb); + */ + /********************** PUBLISHERS **********************/ + // Initiate publisher to publish commanded local position + ros::Publisher local_pos_pub = nh.advertise + ("mavros/setpoint_position/local", 10); + + // Publish desired attitude + ros::Publisher thrust_pub = nh.advertise + ("mavros/setpoint_attitude/thrust", 10); + + // Publish attitude commands + ros::Publisher att_pub = nh.advertise + ("/mavros/setpoint_attitude/attitude",10); + + // Service Clients + ros::ServiceClient arming_client = nh.serviceClient + ("mavros/cmd/arming"); + ros::ServiceClient set_mode_client = nh.serviceClient + ("mavros/set_mode"); + ros::ServiceClient takeoff_cl = nh.serviceClient + ("mavros/cmd/takeoff"); + ros::ServiceClient waypoint_cl = nh.serviceClient + ("status/waypoint_tracker"); + + //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms + ros::Rate rate(20.0); + + // wait for FCU connection + while(ros::ok() && !current_state.connected){ + ros::spinOnce(); + rate.sleep(); + } + + // Retrieve desired waypoints + oscillation_ctrl::WaypointTrack wpoint_srv; + wpoint_srv.request.query = false; + if (waypoint_cl.call(wpoint_srv)) + { + ROS_INFO("Waypoints received"); + } + + // populate desired waypoints + pose.pose.position.x = wpoint_srv.response.xd.x; + pose.pose.position.y = wpoint_srv.response.xd.y; + pose.pose.position.z = wpoint_srv.response.xd.z; + + //send a few setpoints before starting + for(int i = 100; ros::ok() && i > 0; --i){ + local_pos_pub.publish(pose); + ros::spinOnce(); + rate.sleep(); + } + + mavros_msgs::SetMode offb_set_mode; + offb_set_mode.request.custom_mode = "OFFBOARD"; + + mavros_msgs::CommandBool arm_cmd; + arm_cmd.request.value = true; + + ros::Time last_request = ros::Time::now(); + + while(ros::ok()){ + if(current_state.mode != "OFFBOARD" && + (ros::Time::now() - last_request > ros::Duration(7.0))){ + if( set_mode_client.call(offb_set_mode) && + offb_set_mode.response.mode_sent){ + ROS_INFO("OFFBOARD"); + } + last_request = ros::Time::now(); + } else { + if( !current_state.armed && + (ros::Time::now() - last_request > ros::Duration(3.0))){ + if(arming_client.call(arm_cmd) && + arm_cmd.response.success){ + ROS_INFO("ARMED"); + } + last_request = ros::Time::now(); + } + } + // Update desired waypoints + waypoint_cl.call(wpoint_srv); + pose.pose.position.x = wpoint_srv.response.xd.x; + pose.pose.position.y = wpoint_srv.response.xd.y; + pose.pose.position.z = wpoint_srv.response.xd.z; + + // User info + ROS_INFO("Current Altitude: %.2f",mavPose.pose.position.z); + ROS_INFO("Desired Altitude: %.2f",pose.pose.position.z); + ROS_INFO("---------------------------"); + + // Check if we are still connected. Otherwise drone should be booted from offboard mode + if(connection_status.data) { + local_pos_pub.publish(pose); + } + else { + ROS_INFO("Connection lost: landing drone..."); + } + + ros::spinOnce(); + rate.sleep(); + } + + return 0; +} + + diff --git a/src/mocap_path_follow.cpp b/src/mocap_path_follow.cpp new file mode 100644 index 0000000..7d1035c --- /dev/null +++ b/src/mocap_path_follow.cpp @@ -0,0 +1,215 @@ +/** + * @file path_follow.cpp + * @brief Offboard path trajectory tracking +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // for std::abs + +/********* CALLBACK FUNCTIONS **********************/ + +// Callback function which will save the current state of the autopilot. +// Allows to check connection, arming, and Offboard tags*/ +mavros_msgs::State current_state; +bool land = false; +void state_cb(const mavros_msgs::State::ConstPtr& msg){ + current_state = *msg; +} + +// Cb to determine attitude target +mavros_msgs::AttitudeTarget att_targ; +void att_targ_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg){ + att_targ = *msg; +} + +// Cb to recieve pose information +geometry_msgs::PoseStamped buff_pose; +geometry_msgs::Quaternion q_init; +geometry_msgs::PoseStamped mavPose; +bool gps_read = false; +double current_altitude; +void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ + mavPose = *msg; + current_altitude = mavPose.pose.position.z; + while(gps_read == false){ + q_init = mavPose.pose.orientation; + if(q_init.x == 0.0 && q_init.w == 0.0){ + ROS_INFO("Waiting..."); + } else { + buff_pose.pose.orientation.x = 0.0; + buff_pose.pose.orientation.y = 0.0; + buff_pose.pose.orientation.z = 0.0; + buff_pose.pose.orientation.w = 1.0; + gps_read = true; + } + } +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "path_follow"); + ros::NodeHandle nh; + + /********************** SUBSCRIBERS **********************/ + // Get current state + ros::Subscriber state_sub = nh.subscribe + ("mavros/state", 10, state_cb); + + // Get attitude target from klausen control + ros::Subscriber att_target_sub = nh.subscribe + ("command/att_target",10,att_targ_cb); + + // Pose subscriber + ros::Subscriber mavPose_sub = nh.subscribe + ("mavros/local_position/pose",10,mavPose_cb); + + /********************** PUBLISHERS **********************/ + // Initiate publisher to publish commanded local position + ros::Publisher local_pos_pub = nh.advertise + ("mavros/setpoint_position/local", 10); + + // Publish attitude and thrust commands + ros::Publisher att_targ_pub = nh.advertise + ("mavros/setpoint_raw/attitude",10); + + // Service Clients + ros::ServiceClient arming_client = nh.serviceClient + ("mavros/cmd/arming"); + ros::ServiceClient set_mode_client = nh.serviceClient + ("mavros/set_mode"); + ros::ServiceClient takeoff_cl = nh.serviceClient + ("mavros/cmd/takeoff"); + ros::ServiceClient waypoint_cl = nh.serviceClient + ("status/waypoint_tracker"); + + //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms + ros::Rate rate_wait(20.0); + + // wait for FCU connection + while(ros::ok() && !current_state.connected){ + ros::spinOnce(); + ROS_INFO("Waiting for FCU connection"); + rate_wait.sleep(); + } + + if (current_state.connected){ + ROS_INFO("Connected to FCU"); + } else { + ROS_INFO("Never Connected"); + } + + /*********** Initiate variables ************************/ + //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms + ros::Rate rate_pub(25.0); + + // Desired mode is offboard + mavros_msgs::SetMode offb_set_mode; + offb_set_mode.request.custom_mode = "OFFBOARD"; + + // Arm UAV + mavros_msgs::CommandBool arm_cmd; + arm_cmd.request.value = true; + + // Take off command + bool takeoff = false, att_cmds = false; + bool oscillation_damp = true; + + // Keep track of time since requests + ros::Time tkoff_req = ros::Time::now(); + ros::Time last_request = ros::Time::now(); + + //send a few setpoints before starting + for(int i = 100; ros::ok() && i > 0; --i){ + local_pos_pub.publish(buff_pose); + ros::spinOnce(); + ROS_INFO("Publishing position setpoints"); + rate_wait.sleep(); + } + + // Retrieve desired waypoints + oscillation_ctrl::WaypointTrack wpoint_srv; + wpoint_srv.request.query = false; + if (waypoint_cl.call(wpoint_srv)){ + ROS_INFO("Waypoints received"); + // populate desired waypoints - this is only for original hover as + // a change of waypoints is handled by ref_signal.py + buff_pose.pose.position.x = wpoint_srv.response.xd.x; + buff_pose.pose.position.y = wpoint_srv.response.xd.y; + buff_pose.pose.position.z = wpoint_srv.response.xd.z; + } else { + ROS_INFO("NO waypoints received"); + } + double alt_des = wpoint_srv.response.xd.z; // Desired height + while(ros::ok()){ + if(current_state.mode == "AUTO.LAND"){ + land = true; + while(land == true){ + ROS_INFO("Des Altitude: LANDING"); + } + } else { + if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){ + if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ + } else { + ROS_INFO("Could not enter offboard mode"); + } + //last_request = ros::Time::now(); + } else { + if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){ + if( arming_client.call(arm_cmd) && arm_cmd.response.success){ + ROS_INFO("Vehicle armed"); + } + last_request = ros::Time::now(); + } + } + if(current_state.mode == "OFFBOARD" && current_state.armed){ + ROS_INFO_ONCE("Spiri is taking off"); + if(!takeoff){ + tkoff_req = ros::Time::now(); + takeoff = true; + } + } + // Check if we want to use oscillation controller + //if (ros::param::get("/use_ctrl", oscillation_damp) == true){ + if (ros::param::has("/status/use_ctrl")){ + ros::param::get("/status/use_ctrl", oscillation_damp); + if(oscillation_damp){ + ROS_INFO("USING ATTITUDE CTRL"); + att_targ.header.stamp = ros::Time::now(); + att_targ_pub.publish(att_targ); + } else { + // Check if waypoints have changed + if (waypoint_cl.call(wpoint_srv)) + { + // populate desired waypoints + buff_pose.pose.position.x = wpoint_srv.response.xd.x; + buff_pose.pose.position.y = wpoint_srv.response.xd.y; + buff_pose.pose.position.z = wpoint_srv.response.xd.z; + } + local_pos_pub.publish(buff_pose); + ROS_INFO("USING POSITION CTRL"); + } + } + ROS_INFO("Des Altitude: %.2f", alt_des); + ROS_INFO("Cur Altitude: %.2f", current_altitude); + ROS_INFO("---------------------------"); + ros::spinOnce(); + rate_pub.sleep(); + } + } + + return 0; +} \ No newline at end of file diff --git a/src/mocap_runTest.py b/src/mocap_runTest.py new file mode 100755 index 0000000..fdbf9a0 --- /dev/null +++ b/src/mocap_runTest.py @@ -0,0 +1,87 @@ +#! /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.xd = Point() + self.xd.x = 0.0 + self.xd.y = 2.0 + self.xd.z = 1.5 + + # Determine if we want to run test with or without controller + self.change_mode = True # True = Change to oscillation damping mode after wait time + 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 + 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_test = 7.0 - rospy.get_time() + self.t_param + if not time_until_test <= 0.0: + rospy.loginfo('In %.2f\nSending waypoints: x = %.2f y = %.2f z = %.2f',time_until_test,self.xd.x,self.xd.y,self.xd.z) + else: + self.set_waypoint(self.xd) + 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 diff --git a/src/path_follow.cpp b/src/path_follow.cpp index 74d536f..844ff7d 100644 --- a/src/path_follow.cpp +++ b/src/path_follow.cpp @@ -49,11 +49,11 @@ void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ if(q_init.x == 0.0 && q_init.w == 0.0){ ROS_INFO("Waiting..."); } else { + buff_pose.pose.orientation.x = q_init.x; + buff_pose.pose.orientation.y = q_init.y; + buff_pose.pose.orientation.z = q_init.z; + buff_pose.pose.orientation.w = q_init.w; gps_read = true; - buff_pose.pose.orientation.x = q_init.x; - buff_pose.pose.orientation.y = q_init.y; - buff_pose.pose.orientation.z = q_init.z; - buff_pose.pose.orientation.w = q_init.w; } } } @@ -101,7 +101,7 @@ int main(int argc, char **argv) // wait for FCU connection while(ros::ok() && !current_state.connected){ ros::spinOnce(); - ROS_INFO("Waiting for FCU connection"); + ROS_INFO_ONCE("Waiting for FCU connection"); rate_wait.sleep(); } @@ -123,12 +123,10 @@ int main(int argc, char **argv) mavros_msgs::CommandBool arm_cmd; arm_cmd.request.value = true; - // Take off command - bool takeoff = false, att_cmds = false; - bool oscillation_damp = true; + // Boolean to set vehicle into oscillation damp mode + bool oscillation_damp = false; // Keep track of time since requests - ros::Time tkoff_req = ros::Time::now(); ros::Time last_request = ros::Time::now(); //send a few setpoints before starting @@ -157,6 +155,7 @@ int main(int argc, char **argv) while(ros::ok()){ if(gps_read == true){ + ROS_INFO("Entered while loop"); if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){ if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ } else { @@ -174,10 +173,6 @@ int main(int argc, char **argv) if(current_state.mode == "OFFBOARD" && current_state.armed){ ROS_INFO_ONCE("Spiri is taking off"); - if(!takeoff){ - tkoff_req = ros::Time::now(); - takeoff = true; - } } // Check if we want to use oscillation controller @@ -186,30 +181,25 @@ int main(int argc, char **argv) ros::param::get("/status/use_ctrl", oscillation_damp); if(oscillation_damp == true){ ROS_INFO("ATTITUDE CTRL"); + att_targ.header.stamp = ros::Time::now(); + // Publish attitude commands + att_targ_pub.publish(att_targ); + } else { + // Check if waypoints have changed + // For attitude controller, ref_signalGen deals with changes + // in desired waypoints, so we only check if not using controller + if (waypoint_cl.call(wpoint_srv)) + { + // populate desired waypoints + buff_pose.pose.position.x = wpoint_srv.response.xd.x; + buff_pose.pose.position.y = wpoint_srv.response.xd.y; + buff_pose.pose.position.z = wpoint_srv.response.xd.z; + } + ROS_INFO("POSITION CTRL"); + // Publish position setpoints + local_pos_pub.publish(buff_pose); } } - - // Determine which messages to send - // if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff && oscillation_damp){ - // att_targ.header.stamp = ros::Time::now(); - // att_targ_pub.publish(att_targ); - // att_cmds = true; - if(oscillation_damp){ - att_targ.header.stamp = ros::Time::now(); - att_targ_pub.publish(att_targ); - att_cmds = true; - } else { - // Check if waypoints have changed - if (waypoint_cl.call(wpoint_srv)) - { - // populate desired waypoints - this is only for original hover as - buff_pose.pose.position.x = wpoint_srv.response.xd.x; - buff_pose.pose.position.y = wpoint_srv.response.xd.y; - buff_pose.pose.position.z = wpoint_srv.response.xd.z; - } - local_pos_pub.publish(buff_pose); - ROS_INFO("POSITION CTRL"); - } ROS_INFO("Des Altitude: %.2f", alt_des); ROS_INFO("Cur Altitude: %.2f", current_altitude); ROS_INFO("---------------------------"); diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py index 62e7e63..03d3d7c 100755 --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -24,10 +24,10 @@ class DesiredPoint(): class Main: - def __init__(self): + def __init__(self): # rate(s) - rate = 50 # rate for the publisher method, specified in Hz -- 10 Hz + rate = 10 # rate for the publisher method, specified in Hz -- 10 Hz # initialize variables self.tstart = rospy.get_time() # Keep track of the start time @@ -53,7 +53,7 @@ class Main: self.tetherL = self.get_tether() # --------------------------------------------------------------------------------# -# SUBSCRIBERS # +# SUBSCRIBERS # # --------------------------------------------------------------------------------# # Topic, msg type, and class callback method rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) @@ -82,14 +82,15 @@ class Main: self.EPS_I = np.zeros(9) # Epsilon shapefilter # Constants for smooth path generation - self.w_tune = 1 # 1 also works well :) 3.13 works well? ######################################################################### - self.epsilon = 0.7 # Damping ratio + self.w_tune = 1 # also works well :) 3.13 works well? ######################################################################### + self.epsilon = 0.7 # Damping ratio # need exception if we do not have tether: if self.tetherL == 0.0: self.wn = self.w_tune else: self.wn = math.sqrt(9.81/self.tetherL) + # self.wn = 7 self.wd = self.wn*math.sqrt(1 - self.epsilon**2) self.k4 = 4*self.epsilon*self.w_tune @@ -98,7 +99,7 @@ class Main: self.k1 = (self.w_tune**4)/(self.k2*self.k3*self.k4) # For saturation: - self.max = [0, 8, 1.5, 3] #[0, 10, 1.5, 3] + self.max = [0, 3, 1.5, 3] #[0, 5, 1.5, 3] self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack) self.empty_point = Point() # Needed to query waypoint_server @@ -141,7 +142,7 @@ class Main: self.at = 3 # acceleration theshold self.pc = 0.7 # From Klausen 2017 self.sk = self.SF_delay_x.shape[1] # from Klausen 2017 - self.ak = np.zeros([self.sk]) + self.ak = np.zeros(self.sk) self.s_gain = 0.0 # Gain found from sigmoid self.service_list = rosservice.get_service_list() @@ -217,7 +218,7 @@ class Main: pos,vel,acc,jer = y error = xd - pos - if abs(error) <= 0.25: error = 0.0 + if abs(error) <= 0.01: error = 0.0 # Derivative of statesape array: dydt = [vel, acc, jer, @@ -304,25 +305,25 @@ class Main: # SOLVE ODE (get ref pos, vel, accel) self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd.x,)) self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd.y,)) - #self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd.z,)) + self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd.z,)) for i in range(1,len(self.y0)): self.x[:,i] = np.clip(self.x[:,i], a_min = -self.max[i], a_max = self.max[i]) self.y[:,i] = np.clip(self.y[:,i], a_min = -self.max[i], a_max = self.max[i]) - #self.z[:,i] = np.clip(self.z[:,i], a_min = -self.max[i], a_max = self.max[i]) + self.z[:,i] = np.clip(self.z[:,i], a_min = -self.max[i], a_max = self.max[i]) for j in range(3): # 3 is due to pos, vel, acc. NOT due x, y, z self.delay(j,SHAPEFIL) # Determine the delay (shapefilter) array + self.EPS_I[3*j+2] = self.z[-1,j] # No need to convolute z-dim if self.SF_idx < len(self.SF_delay_x[0]): self.EPS_I[3*j] = self.x[-1,j] self.EPS_I[3*j+1] = self.y[-1,j] - #self.EPS_I[3*j+2] = self.z[1,j] + else: self.EPS_I[3*j] = self.A1*self.x[-1,j] + self.A2*self.SF_delay_x[j,0] # Determine convolution (x) - self.EPS_I[3*j+1] = self.A1*self.y[-1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y) - #self.EPS_I[3*j+2] = self.z[1,j] # No need to convolute z-dim + self.EPS_I[3*j+1] = self.A1*self.y[-1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y) self.delay(1,FEEDBACK) # Detemine feedback array @@ -345,18 +346,19 @@ class Main: self.ref_sig.acceleration.y = self.EPS_F[7] # Do not need to evaluate z - self.ref_sig.position.z = self.xd.z - self.ref_sig.velocity.z = 0.0 - self.ref_sig.acceleration.z = 0.0 - #self.ref_sig.position.z = self.EPS_F[2] - #self.ref_sig.velocity.z = self.EPS_F[5] - #self.ref_sig.acceleration.z = self.EPS_F[8] + # self.ref_sig.position.z = self.xd.z + # self.ref_sig.velocity.z = 0.0 + # self.ref_sig.acceleration.z = 0.0 + self.ref_sig.position.z = self.EPS_F[2] + self.ref_sig.velocity.z = self.EPS_F[5] + self.ref_sig.acceleration.z = self.EPS_F[8] # self.x0 = [self.dr_pos.position.x, self.x[1,1], self.x[1,2], self.x[1,3]] # self.y0 = [self.dr_pos.position.y, self.y[1,1], self.y[1,2], self.y[1,3]] # self.z0 = [self.dr_pos.position.z, self.z[1,1], self.z[1,2], self.z[1,3]] self.x0 = [self.x[1,0], self.x[1,1], self.x[1,2], self.x[1,3]] - self.y0 = [self.y[1,0], self.y[1,1], self.y[1,2], self.y[1,3]] + self.y0 = [self.y[1,0], self.y[1,1], self.y[1,2], self.y[1,3]] + self.z0 = [self.z[1,0], self.z[1,1], self.z[1,2], self.z[1,3]] self.SF_idx += 1 self.FB_idx += 1 @@ -377,15 +379,15 @@ class Main: return EPS_D - def screen_output(self): + def user_fback(self): # Feedback to user - # rospy.loginfo(' Var | x | y | z ') - # rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2]) - # rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5]) - # rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8]) - # rospy.loginfo('_______________________') - rospy.loginfo_once('Tether length: %.2f',self.tetherL) + rospy.loginfo(' Var | x | y | z ') + rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2]) + rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5]) + rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8]) + rospy.loginfo('_______________________') + # rospy.loginfo_once('Tether length: %.2f',self.tetherL) # rospy.loginfo('Theta: %.2f',self.load_angles.theta) # rospy.loginfo('Phi: %.2f',self.load_angles.phi) @@ -398,7 +400,7 @@ class Main: self.pub_path.publish(self.ref_sig) # Give user feedback on published message: - self.screen_output() + self.user_fback() if __name__=="__main__":