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__":