From 937fd75fee01180b71c0e3f9a76df48c3fc8e540 Mon Sep 17 00:00:00 2001 From: cesar Date: Wed, 14 Sep 2022 14:19:12 -0300 Subject: [PATCH] Lastest changes to algorithm + adding airframes to work with Ubuntu 20.04 --- .gitignore | 3 + CMakeLists.txt | 6 +- airframes/18.04/4000_spiri | 25 +++ airframes/18.04/4001_spiri_with_tether | 26 +++ airframes/{ => 20.04}/4000_spiri | 0 airframes/{ => 20.04}/4001_spiri_with_tether | 8 +- config/Ctrl_param.yaml | 14 -- config/gazebo_config.yaml | 6 - config/mocapGazebo_params.yaml | 14 ++ config/mocapLab_params.yaml | 27 +++ config/noCtrl_param.yaml | 7 - config/spiri_params.yaml | 9 + launch/headless_spiri_mocap.launch | 54 +++++ .../headless_spiri_with_tether_mocap.launch | 54 +++++ launch/mocap_oscillation_damp.launch | 7 +- launch/mocap_sim.launch | 91 ++++---- launch/mocap_takeoff_noCtrl.launch | 65 ------ launch/oscillation_damp.launch | 7 +- src/LinkState.py | 47 +---- src/MoCap_Localization_noTether.py | 183 ----------------- src/klausen_control.py | 60 ++---- src/mocap_offb_node.cpp | 194 ------------------ src/mocap_runTest.py | 37 +++- src/offb_node.cpp | 185 ----------------- src/path_follow.cpp | 101 ++++----- src/ref_signalGen.py | 44 ++-- src/set_ploadmass.py | 59 ++++++ src/thrust_controller.py | 173 ---------------- 28 files changed, 436 insertions(+), 1070 deletions(-) create mode 100644 airframes/18.04/4000_spiri create mode 100644 airframes/18.04/4001_spiri_with_tether rename airframes/{ => 20.04}/4000_spiri (100%) rename airframes/{ => 20.04}/4001_spiri_with_tether (85%) delete mode 100644 config/Ctrl_param.yaml delete mode 100644 config/gazebo_config.yaml create mode 100644 config/mocapGazebo_params.yaml create mode 100644 config/mocapLab_params.yaml delete mode 100644 config/noCtrl_param.yaml create mode 100644 config/spiri_params.yaml create mode 100644 launch/headless_spiri_mocap.launch create mode 100644 launch/headless_spiri_with_tether_mocap.launch delete mode 100644 launch/mocap_takeoff_noCtrl.launch delete mode 100755 src/MoCap_Localization_noTether.py delete mode 100644 src/mocap_offb_node.cpp delete mode 100644 src/offb_node.cpp create mode 100755 src/set_ploadmass.py delete mode 100755 src/thrust_controller.py diff --git a/.gitignore b/.gitignore index 40f8a97..065dea8 100644 --- a/.gitignore +++ b/.gitignore @@ -2,10 +2,13 @@ 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 e1717b5..1d72db3 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/airframes/18.04/4000_spiri b/airframes/18.04/4000_spiri new file mode 100644 index 0000000..81ad67a --- /dev/null +++ b/airframes/18.04/4000_spiri @@ -0,0 +1,25 @@ +#!/bin/sh +# +# @name Generic Quadcopter +# +# @type Quadrotor x +# @class Copter + +. ${R}etc/init.d/rc.mc_defaults + +param set-default MC_ROLLRATE_K 2.35 +param set-default MC_ROLLRATE_D 0.0032 +param set-default MC_ROLLRATE_I 0.15 + +param set-default MC_PITCHRATE_K 2.35 +param set-default MC_PITCHRATE_D 0.0032 +param set-default MC_PITCHRATE_I 0.15 + +param set-default MPC_Z_VEL_MAX_UP 1.0 + +param set EKF2_AID_MASK 1 +param set EKF2_HGT_MODE 0 +param set SYS_FAILURE_EN 0 + +set MIXER quad_x +set PWM_OUT 1234 diff --git a/airframes/18.04/4001_spiri_with_tether b/airframes/18.04/4001_spiri_with_tether new file mode 100644 index 0000000..0bb23c7 --- /dev/null +++ b/airframes/18.04/4001_spiri_with_tether @@ -0,0 +1,26 @@ +#!/bin/sh +# +# @name Generic Quadcopter +# +# @type Quadrotor x +# @class Copter + +. ${R}etc/init.d/rc.mc_defaults + +param set MC_ROLLRATE_K 2.35 +param set MC_ROLLRATE_D 0.0030 +param set MC_ROLLRATE_I 0.15 + +param set MC_PITCHRATE_K 2.35 +param set MC_PITCHRATE_D 0.0030 +param set MC_PITCHRATE_I 0.15 + +param set MPC_Z_VEL_MAX_UP 0.5 +param set MPC_TKO_SPEED 1.0 + +param set EKF2_AID_MASK 1 +param set EKF2_HGT_MODE 0 +param set SYS_FAILURE_EN 0 + +set MIXER quad_x +set PWM_OUT 1234 diff --git a/airframes/4000_spiri b/airframes/20.04/4000_spiri similarity index 100% rename from airframes/4000_spiri rename to airframes/20.04/4000_spiri diff --git a/airframes/4001_spiri_with_tether b/airframes/20.04/4001_spiri_with_tether similarity index 85% rename from airframes/4001_spiri_with_tether rename to airframes/20.04/4001_spiri_with_tether index f419a52..07334d2 100644 --- a/airframes/4001_spiri_with_tether +++ b/airframes/20.04/4001_spiri_with_tether @@ -16,11 +16,6 @@ param set-default MC_PITCHRATE_D 0.0060 param set-default MC_PITCHRATE_I 0.35 param set-default MPC_Z_P 0.70 - -#param load spiri_param/Vehicle_230_Parameters.params - -#param set-default MC_PITCHRATE_P 0.0889 -#param set-default MC_ROLLRATE_P 0.0957 param set-default CA_AIRFRAME 0 param set-default CA_ROTOR_COUNT 4 @@ -45,8 +40,7 @@ param set-default COM_RCL_EXCEPT 4 param set-default MPC_Z_VEL_MAX_UP 1.0 -param set-default COM_RCL_EXCEPT 4 -param set-default NAV_RCL_ACT 1 +param set-default COM_RCL_EXCEPT 4 # Ignores no RC failsafe (not needed for simulations) set MIXER quad_x set PWM_OUT 1234 diff --git a/config/Ctrl_param.yaml b/config/Ctrl_param.yaml deleted file mode 100644 index e50ad51..0000000 --- a/config/Ctrl_param.yaml +++ /dev/null @@ -1,14 +0,0 @@ -# Ros param when using Klausen Ctrl -wait_time: 30 -#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 deleted file mode 100644 index 4148cfa..0000000 --- a/config/gazebo_config.yaml +++ /dev/null @@ -1,6 +0,0 @@ -# Ros param when not using Klausen Ctrl -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/config/mocapGazebo_params.yaml b/config/mocapGazebo_params.yaml new file mode 100644 index 0000000..79c4ac5 --- /dev/null +++ b/config/mocapGazebo_params.yaml @@ -0,0 +1,14 @@ +# Ros param when using Klausen Ctrl +wait_time: 30 # parameter which can be set to run desired tests at a desired time +#drone_mass: 0.614 # weight with new battery +drone_mass: 0.602 +pload_mass: 0.1 # mass of payload. Needs to be changed in spiri_with_tether file as well +#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 # starts PX4 without attitude controller +waypoints: {x: 0.0, y: 0.0, z: 5.0} # takeoff waypoints +# sets waypoints to run a square test +square_x: [0.5,1,1,1,0.5,0,0] +square_y: [0,0,0.5,1,1,1,0.5] diff --git a/config/mocapLab_params.yaml b/config/mocapLab_params.yaml new file mode 100644 index 0000000..eae0f55 --- /dev/null +++ b/config/mocapLab_params.yaml @@ -0,0 +1,27 @@ +# Ros param when using Klausen Ctrl +wait_time: 30 # parameter which can be set to run desired tests at a desired time + +# DRONE MASSES +#drone_mass: 0.614 # weight with new battery +drone_mass: 0.602 + +#PLOAD MASSES +#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 + +# CTRL PARAMETER - should be false to start always +use_ctrl: false # starts PX4 without attitude controller + +waypoints: {x: 0.0, y: 0.0, z: 1.75} # takeoff waypoints + +# sets waypoints to run a square test +square_x: [0.5,1,1,1,0.5,0,0] +square_y: [0,0,0.5,1,1,1,0.5] + +# HOVER THROTTLE - Changes depending on mass of pload and drone +# hover_throttle: 0.32 # Hover throttle with pload 0.15 kg +hover_throttle: 0.28 # Hover throttle with pload 0.10 kg +# hover_throttle: 0.22 # Hover throttle with no pload +#hover_throttle: 0.26 # Hover throttle with pload 0.05 kg diff --git a/config/noCtrl_param.yaml b/config/noCtrl_param.yaml deleted file mode 100644 index 4d81a8c..0000000 --- a/config/noCtrl_param.yaml +++ /dev/null @@ -1,7 +0,0 @@ -# Ros param when not using Klausen Ctrl -wait_time: 40 -drone_mass: 0.5841 -#drone_mass: 1.437 -pload_mass: 0.10 - - diff --git a/config/spiri_params.yaml b/config/spiri_params.yaml new file mode 100644 index 0000000..7255a5c --- /dev/null +++ b/config/spiri_params.yaml @@ -0,0 +1,9 @@ +# Set useful ROS parameters for simulation +wait_time: 30 # parameter which can be set to run desired tests at a desired time +drone_mass: 1.437 # mass of drone +pload_mass: 0.25 # mass of payload +use_ctrl: false # starts PX4 without attitude controller - needs to be set to false to takeoff +waypoints: {x: 0.0, y: 0.0, z: 5.0} # takeoff waypoints +# sets waypoints to run a square test +square_x: [0.5,1,1,1,0.5,0,0] +square_y: [0,0,0.5,1,1,1,0.5] diff --git a/launch/headless_spiri_mocap.launch b/launch/headless_spiri_mocap.launch new file mode 100644 index 0000000..0ed8922 --- /dev/null +++ b/launch/headless_spiri_mocap.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/headless_spiri_with_tether_mocap.launch b/launch/headless_spiri_with_tether_mocap.launch new file mode 100644 index 0000000..1c0bb7e --- /dev/null +++ b/launch/headless_spiri_with_tether_mocap.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/mocap_oscillation_damp.launch b/launch/mocap_oscillation_damp.launch index 4a86ca8..a03d9d3 100644 --- a/launch/mocap_oscillation_damp.launch +++ b/launch/mocap_oscillation_damp.launch @@ -14,13 +14,8 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo - - - - - - + diff --git a/launch/mocap_sim.launch b/launch/mocap_sim.launch index 9a353bb..6cd2bdd 100644 --- a/launch/mocap_sim.launch +++ b/launch/mocap_sim.launch @@ -3,12 +3,10 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo /--> - - - 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 deleted file mode 100644 index bc2574a..0000000 --- a/launch/mocap_takeoff_noCtrl.launch +++ /dev/null @@ -1,65 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/oscillation_damp.launch b/launch/oscillation_damp.launch index 40aaaf2..e811eeb 100644 --- a/launch/oscillation_damp.launch +++ b/launch/oscillation_damp.launch @@ -5,11 +5,9 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo - - - + - + @@ -37,7 +35,6 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo pkg="oscillation_ctrl" type="klausen_control.py" name="klausenCtrl_node" - launch-prefix="xterm -e" /> 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,orientation): """ @@ -171,7 +147,6 @@ class Main: # Determine thetadot self.loadAngles.thetadot = (self.loadAngles.theta - self.thetabuf)/self.dt - self.loadAngles.thetadot = self.cutoff(self.loadAngles.thetadot,self.thetadot_max) self.thetabuf = self.loadAngles.theta # Determine phi (roll) @@ -184,7 +159,6 @@ class Main: # Determine phidot self.loadAngles.phidot = (self.loadAngles.phi - self.phibuf)/self.dt - self.loadAngles.phidot = self.cutoff(self.loadAngles.phidot,self.phidot_max) self.phibuf = self.loadAngles.phi # Update buffer else: # Otherwise, vars = 0 @@ -204,17 +178,18 @@ class Main: rospy.loginfo("Get Link State call failed: {0}".format(e)) def user_feedback(self,gui_timer): - # Print and save results - #print "\n" + print ("\n") rospy.loginfo("") - 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)) - 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 "phi: " + str(round(self.loadAngles.phi*180/3.14,3)) - print "theta: " + str(round(self.loadAngles.theta*180/3.14,3)) + 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() diff --git a/src/MoCap_Localization_noTether.py b/src/MoCap_Localization_noTether.py deleted file mode 100755 index 1fcd148..0000000 --- a/src/MoCap_Localization_noTether.py +++ /dev/null @@ -1,183 +0,0 @@ -#!/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 6831ea8..d8307ec 100755 --- a/src/klausen_control.py +++ b/src/klausen_control.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2.7 +#!/usr/bin/env python ### Cesar Rodriguez Aug 2021 ### Trajectory controller @@ -96,12 +96,10 @@ class Main: self.z2 = np.zeros([5,1]) # [vx;vy;vz;thetadot;phidot] - alpha # Tuning gains - 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]) + self.K1 = np.identity(3) + self.K2 = np.identity(5) + self.tune = 0.1 # Tuning parameter + self.dist = np.array([0,0,0,0.1,0.1]) # Wind disturbance # Gain terms self.Kp = np.identity(3) + np.dot(self.K2[:3,:3],self.K1) + self.tune*np.identity(3) self.Kd = self.tot_m*self.K1 + self.tune*self.K2[:3,:3] @@ -109,10 +107,8 @@ class Main: # PD Thrust Controller terms # gains for thrust PD Controller - #self.Kp = 3.0 - #self.Kd = 3 - self.Kp_thrust = 1.5 #3.0 #1.5 - self.Kd_thrust = 1.0 #3.0 # 1.0 + self.Kp_thrust = 1.5 + self.Kd_thrust = 1.0 self.R = np.empty([3,3]) # rotation matrix self.e3 = np.array([[0],[0],[1]]) # Get scaling thrust factor, kf @@ -125,7 +121,6 @@ class Main: # Topic, msg type, and class callback method rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) rospy.Subscriber('/reference/path', RefSignal, self.refsig_cb) - # rospy.Subscriber('/status/twoBody_status', TetheredStatus, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) @@ -187,13 +182,20 @@ class Main: return [drone_m, pl_m] def get_kf(self): - if rospy.has_param('mocap/hover_throttle'): - hover_throttle = rospy.get_param('mocap/hover_throttle') + if rospy.has_param('status/hover_throttle'): + hover_throttle = rospy.get_param('status/hover_throttle') else: hover_throttle = (self.tot_m*9.81 + 9.57)/34.84 # linear scaling depending on dependent on mass kf = hover_throttle/(self.tot_m*9.81) return kf + # Check if vehicle has tether + def tether_check(self): + if self.tether == True: + rospy.loginfo_once('TETHER LENGTH: %.2f', self.tetherL) + else: + rospy.loginfo_once('NO TETHER DETECTED') + # --------------------------------------------------------------------------------# # CALLBACK FUNCTIONS # --------------------------------------------------------------------------------# @@ -243,13 +245,6 @@ class Main: except ValueError: pass - # Check if vehicle has tether - def tether_check(self): - if self.tether == True: - rospy.loginfo_once('TETHER LENGTH: %.2f', self.tetherL) - else: - rospy.loginfo_once('NO TETHER DETECTED') - def waypoints_srv_cb(self): if '/status/waypoint_tracker' in self.service_list: rospy.wait_for_service('/status/waypoint_tracker') @@ -358,17 +353,9 @@ class Main: # determine Rotation Matrix self.R_e3 = np.array([[self.R.T[2][0]],[self.R.T[2][1]],[self.R.T[2][2]]]) - # test which one is better: - # thrust_vector = (9.81*self.tot_m*self.e3 + self.Kp_thrust*self.error + self.Kd_thrust*self.error_vel - self.tot_m*self.path_acc)*self.kf - # 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.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)) @@ -458,16 +445,11 @@ class Main: 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*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" + + # Update self.z1_p for integration self.z1_p = self.z1 # Covert Fd into drone frame @@ -479,11 +461,9 @@ class Main: 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]) - q = quaternion_from_euler(self.EulerAng[0],self.EulerAng[1],self.EulerAng[2]) - self.user_fback(Fd,Fd_tf) + self.user_feedback(Fd,Fd_tf) # Populate msg variable # Attitude control @@ -495,7 +475,7 @@ class Main: self.att_targ.orientation.z = q[2] self.att_targ.orientation.w = q[3] - def user_fback(self,F_noTransform, F_Transform): + def user_feedback(self,F_noTransform, F_Transform): print('\n') rospy.loginfo('thrust: %.6f' % self.att_targ.thrust) rospy.loginfo('roll: %.2f pitch: %.2f',self.EulerAng[0]*180/3.14,self.EulerAng[1]*180/3.14) diff --git a/src/mocap_offb_node.cpp b/src/mocap_offb_node.cpp deleted file mode 100644 index ad53acc..0000000 --- a/src/mocap_offb_node.cpp +++ /dev/null @@ -1,194 +0,0 @@ -/** - * @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_runTest.py b/src/mocap_runTest.py index fdbf9a0..77c87af 100755 --- a/src/mocap_runTest.py +++ b/src/mocap_runTest.py @@ -20,13 +20,23 @@ class Main: 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 + 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.' @@ -49,6 +59,7 @@ class Main: """ 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: @@ -58,14 +69,20 @@ class Main: 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) + 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 """ diff --git a/src/offb_node.cpp b/src/offb_node.cpp deleted file mode 100644 index 50cecbd..0000000 --- a/src/offb_node.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/** - * @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 - - -/********* 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 { - pose_read = true; - // pose.pose.orientation.x = q_init.x; - // pose.pose.orientation.y = q_init.y; - // pose.pose.orientation.z = q_init.z; - // pose.pose.orientation.w = q_init.w; - } - } - -} - -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); - - // Waypoint Subscriber - /* - ros::Subscriber waypoint_sub = nh.subscribe - ("/reference/waypoints",10,waypoints_cb); - */ - ros::ServiceClient waypoint_cl = nh.serviceClient - ("status/waypoint_tracker"); - - /********************** 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"); - - //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; - - /*/ Populate pose msg - pose.pose.orientation.x = q_init.x; - pose.pose.orientation.y = q_init.y; - pose.pose.orientation.z = q_init.z; - pose.pose.orientation.w = q_init.w; - */ - - //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(5.0))){ - if( set_mode_client.call(offb_set_mode) && - offb_set_mode.response.mode_sent){ - ROS_INFO("Offboard enabled"); - } - 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("Vehicle armed"); - } - last_request = ros::Time::now(); - } - } - // check if waypoints have changed 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("---------------------------"); - - local_pos_pub.publish(pose); - - ros::spinOnce(); - rate.sleep(); - } - - return 0; -} - - diff --git a/src/path_follow.cpp b/src/path_follow.cpp index 844ff7d..1b922f7 100644 --- a/src/path_follow.cpp +++ b/src/path_follow.cpp @@ -44,18 +44,18 @@ 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 = 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; - } - } + // 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 = 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; + // } + // } } int main(int argc, char **argv) @@ -154,58 +154,47 @@ int main(int argc, char **argv) double alt_des = wpoint_srv.response.xd.z; // Desired height 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 { - ROS_INFO("Could not enter offboard mode"); - } - //last_request = ros::Time::now(); + 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 { - 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(); + ROS_INFO("Could not enter offboard mode"); + } + } 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(current_state.mode == "OFFBOARD" && current_state.armed){ + ROS_INFO_ONCE("Spiri is taking off"); + } - // 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 == 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); - } + // Check if we want to use oscillation controller + if (ros::param::has("/status/use_ctrl")){ + 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 { + ROS_INFO("POSITION CTRL"); + //ROS_INFO("POSITION CTRL"); + // Publish position setpoints + local_pos_pub.publish(buff_pose); } - ROS_INFO("Des Altitude: %.2f", alt_des); + ROS_INFO("Des Altitude: %.2f", wpoint_srv.response.xd.z); ROS_INFO("Cur Altitude: %.2f", current_altitude); ROS_INFO("---------------------------"); - ros::spinOnce(); - rate_pub.sleep(); + } else { + ROS_WARN("CONTROL PARAM NOT FOUND. PLEASE SET '/status/use_ctrl'"); } + + ros::spinOnce(); + rate_pub.sleep(); } return 0; diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py index 03d3d7c..a3d71e0 100755 --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2.7 +#!/usr/bin/env python ### Cesar Rodriguez July 2021 ### Based off of Klausen 2017 - Smooth trajectory generation based on desired waypoints @@ -10,11 +10,10 @@ import math import rosservice from scipy.integrate import odeint -from oscillation_ctrl.msg import RefSignal, LoadAngles, TetheredStatus +from oscillation_ctrl.msg import RefSignal, LoadAngles from oscillation_ctrl.srv import WaypointTrack from geometry_msgs.msg import Pose, PoseStamped, Point, TwistStamped from sensor_msgs.msg import Imu -from mavros_msgs.msg import State class DesiredPoint(): def __init__(self,x,y,z): @@ -38,12 +37,11 @@ class Main: self.tmax = self.dt # used to get desired pos, vel, and acc for next time step (tmax) self.n = self.tmax/self.dt + 1 self.t = np.linspace(0, self.tmax, self.n) # Time array - # self.t = np.linspace(0, 1.0, 10) # Time array # Message generation/ collection self.vel_data = TwistStamped() # This is needed to get drone vel from gps - self.imu_data = Imu() # Needed for to get drone acc from IMU - self.ref_sig = RefSignal() # Smooth Signal + self.imu_data = Imu() # Needed for to get drone acc from IMU + self.ref_sig = RefSignal() # Smooth Signal self.load_angles = LoadAngles() self.has_run = 0 # Bool to keep track of first run instance @@ -58,7 +56,6 @@ class Main: # Topic, msg type, and class callback method rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb) - # rospy.Subscriber('/status/twoBody_status', TetheredStatus, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) @@ -67,8 +64,7 @@ class Main: # --------------------------------------------------------------------------------# # Publish desired path to compute attitudes self.pub_path = rospy.Publisher('/reference/path',RefSignal,queue_size = 10) - # Needed for geometric controller to compute thrust - #self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10) + self.waypointTracker_pub = rospy.Publisher('/reference/waypoints',Point,queue_size = 10) # not needed. Used for performance analysis # timer(s), used to control method loop freq(s) as defined by the rate(s) self.pub_tim = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) @@ -82,7 +78,7 @@ class Main: self.EPS_I = np.zeros(9) # Epsilon shapefilter # Constants for smooth path generation - self.w_tune = 1 # also works well :) 3.13 works well? ######################################################################### + self.w_tune = 1 # Increase this to increase aggresiveness of trajectory i.e. higher accelerations self.epsilon = 0.7 # Damping ratio # need exception if we do not have tether: @@ -330,9 +326,6 @@ class Main: self.sigmoid() # Determine sigmoid gain EPS_D = self.fback() # Feedback Epsilon - # for i in range(9): - # Populate EPS_F buffer with desired change based on feedback - # self.EPS_F[i] = self.EPS_I[i] + self.s_gain*EPS_D[i] #+ EPS_D[i] self.EPS_F = self.EPS_I + self.s_gain*EPS_D # Populate msg with epsilon_final @@ -340,22 +333,14 @@ class Main: #self.ref_sig.type_mask = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin self.ref_sig.position.x = self.EPS_F[0] self.ref_sig.position.y = self.EPS_F[1] + self.ref_sig.position.z = self.EPS_F[2] self.ref_sig.velocity.x = self.EPS_F[3] self.ref_sig.velocity.y = self.EPS_F[4] + self.ref_sig.velocity.z = self.EPS_F[5] self.ref_sig.acceleration.x = self.EPS_F[6] 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.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.z0 = [self.z[1,0], self.z[1,1], self.z[1,2], self.z[1,3]] @@ -379,7 +364,7 @@ class Main: return EPS_D - def user_fback(self): + def user_feeback(self): # Feedback to user rospy.loginfo(' Var | x | y | z ') @@ -387,9 +372,7 @@ class Main: 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) + def publisher(self,pub_tim): @@ -399,8 +382,11 @@ class Main: # Publish reference signal self.pub_path.publish(self.ref_sig) + # Publish what the setpoints are + self.waypointTracker_pub.publish(self.xd) + # Give user feedback on published message: - self.user_fback() + self.user_feeback() if __name__=="__main__": diff --git a/src/set_ploadmass.py b/src/set_ploadmass.py new file mode 100755 index 0000000..347ef17 --- /dev/null +++ b/src/set_ploadmass.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python2.7 + +### Cesar Rodriguez Sept 2022 +### changes pload mass depending on /status/pload_mass parameter + +import rospy +import rosservice +from gazebo_msgs.srv import GetLinkProperties, SetLinkProperties + +class Main: + def __init__(self): + # Variables needed for testing start + + # link (payload) name + self.link_name = 'spiri_with_tether::mass::payload' + + # service names we will use + self.service_request = '/gazebo/get_link_properties' + self.service_call = '/gazebo/set_link_properties' + + # wait for service + rospy.wait_for_service(self.service_request) + # set up service request + get_link_properties = rospy.ServiceProxy(self.service_request,GetLinkProperties) + # wait for service to set link properties + rospy.wait_for_service(self.service_call) + # set up service request + set_link_properties = rospy.ServiceProxy(self.service_call,SetLinkProperties) + + # get payload properties + self.pload_properties = get_link_properties(self.link_name) + + # state parameter name for where payload mass is stored + self.param_pload_mass = 'status/pload_mass' + + # check if parameter exists + if rospy.has_param(self.param_pload_mass): + self.desired_mass = rospy.get_param(self.param_pload_mass) # get desired mass + try: + # set desired mass and necessary inertial properties + set_link_properties(link_name=self.link_name,mass=self.desired_mass, + ixx=self.pload_properties.ixx,iyy=self.pload_properties.iyy,izz=self.pload_properties.izz, + gravity_mode=self.pload_properties.gravity_mode) + rospy.loginfo('Set payload mass to: %.3f', self.desired_mass) + except rospy.ServiceException as e: + rospy.loginfo("Set Link State call failed: {0}".format(e)) + else: rospy.logwarn('COULD NOT FIND PLOAD MASS PARAM') + +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/thrust_controller.py b/src/thrust_controller.py deleted file mode 100755 index 0c56345..0000000 --- a/src/thrust_controller.py +++ /dev/null @@ -1,173 +0,0 @@ -#!/usr/bin/env python2.7 - -### Cesar Rodriguez Sept 21 -### Used to control thrust of drone - -import rospy, tf -import numpy as np - -from geometry_msgs.msg import PoseStamped, TwistStamped -from mavros_msgs.msg import AttitudeTarget - -class PID: - def __init__(self): - # rate(s) - rate = 35.0 - self.dt = 1/rate - # 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() - - # tuning gains - #self.Kp = 0.335 - #self.Kd = 0.1 - self.Kp = 2.7 - self.Kd = 0.5 - - # drone var - self.drone_m = 1.437 - self.max_a = 14.2 - self.max_t = self.drone_m*self.max_a - - # message variables - self.pose = PoseStamped() - self.pose_buff = PoseStamped() - self.pose_buff.pose.position.z = 2.5 - - self.attitude = AttitudeTarget() - self.attitude.type_mask = 1|2|4 # ignore body rate command - self.attitude.orientation.x = 0.0 - self.attitude.orientation.y = 0.0 - self.attitude.orientation.z = 0.0 - self.attitude.orientation.w = 1.0 - - self.des_alt = 2.5 - self.dr_vel = TwistStamped() - #self.cur_att = PoseStamped() - self.R = np.empty([3,3]) - - # clients - - - # subscribers - rospy.Subscriber('/mavros/local_position/pose',PoseStamped, self.pose_cb) - rospy.Subscriber('/mavros/local_position/velocity_local', TwistStamped, self.droneVel_cb) - - # publishers - self.pub_attitude = rospy.Publisher('/command/thrust', AttitudeTarget, queue_size=10) - - - # publishing rate - self.pub_time = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) - - def pose_cb(self,msg): - self.cur_alt = msg.pose.position.z - - self.pose = msg - - # Callback for drone velocity - def droneVel_cb(self,msg): - try: - self.dr_vel = msg - - except ValueError or TypeError: - pass - - - def quaternion_rotation_matrix(self): - """ - Covert a quaternion into a full three-dimensional rotation matrix. - - Input - :param Q: A 4 element array representing the quaternion (q0,q1,q2,q3) - - Output - :return: A 3x3 element matrix representing the full 3D rotation matrix. - This rotation matrix converts a point in the local reference - frame to a point in the global reference frame. - """ - # Extract the values from Q - q0 = self.pose.pose.orientation.w - q1 = self.pose.pose.orientation.x - q2 = self.pose.pose.orientation.y - q3 = self.pose.pose.orientation.z - - # First row of the rotation matrix - r00 = 2 * (q0 * q0 + q1 * q1) - 1 - r01 = 2 * (q1 * q2 - q0 * q3) - r02 = 2 * (q1 * q3 + q0 * q2) - - # Second row of the rotation matrix - r10 = 2 * (q1 * q2 + q0 * q3) - r11 = 2 * (q0 * q0 + q2 * q2) - 1 - r12 = 2 * (q2 * q3 - q0 * q1) - - # Third row of the rotation matrix - r20 = 2 * (q1 * q3 - q0 * q2) - r21 = 2 * (q2 * q3 + q0 * q1) - r22 = 2 * (q0 * q0 + q3 * q3) - 1 - - # 3x3 rotation matrix - rot_matrix = np.array([[r00, r01, r02], - [r10, r11, r12], - [r20, r21, r22]]) - - return rot_matrix - - def operation(self): - self.error = self.cur_alt - self.des_alt # error in z dir. - self.R = self.quaternion_rotation_matrix() # determine Rotation Matrix - - # thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3) - # Taeyoung Lee, Melvin Leok, and N. Harris McClamroch - thrust = np.dot(9.81*self.drone_m -self.Kp*self.error -self.Kd*self.dr_vel.twist.linear.z,self.R.T[2])/self.max_t - self.attitude.thrust = thrust[2] # save thurst in z-dir - - # Value needs to be between 0 - 1.0 - if self.attitude.thrust >= 1.0: - self.attitude.thrust = 1.0 - elif self.attitude.thrust <= 0.0: - self.attitude.thrust = 0.0 - - - def publisher(self,pub_time): - self.operation() # determine thrust - self.attitude.header.stamp = rospy.Time.now() - self.pub_attitude.publish(self.attitude) - - def user_feedback(self): - rospy.loginfo('Des: %.2f',self.des_alt) - rospy.loginfo('Error: %.2f',self.error) - rospy.loginfo('Throttle: %f\n',self.attitude.thrust) - -if __name__=="__main__": - - # Initiate ROS node - rospy.init_node('thrust_PID') - try: - PID() # create class object - - rospy.spin() # loop until shutdown signal - - except rospy.ROSInterruptException: - pass - - - - - - - - - - - - - - - - - - -