diff --git a/config/px4_config.yaml b/config/px4_config.yaml
index 8a0d62f..5d1dbb2 100644
--- a/config/px4_config.yaml
+++ b/config/px4_config.yaml
@@ -208,7 +208,7 @@ landing_target:
# mocap_pose_estimate
mocap:
# select mocap source
- use_tf: false # ~mocap/tf
+ use_tf: false # ~mocap/tf
use_pose: true # ~mocap/pose
# odom
diff --git a/config/px4_pluginlists.yaml b/config/px4_pluginlists.yaml
index f9087de..5a97f05 100644
--- a/config/px4_pluginlists.yaml
+++ b/config/px4_pluginlists.yaml
@@ -2,14 +2,18 @@ plugin_blacklist:
# common
- safety_area
# extras
+- tdr_radio
- image_pub
- vibration
- distance_sensor
- rangefinder
- wheel_odometry
- mission
-- tdr_radio
- fake_gps
+- setpoint_accel
+- landing_target
+- odometry
+- px4flow
plugin_whitelist: []
#- 'sys_*'
diff --git a/launch/cortex_bridge.launch b/launch/cortex_bridge.launch
index 256d668..d2b325e 100644
--- a/launch/cortex_bridge.launch
+++ b/launch/cortex_bridge.launch
@@ -9,10 +9,12 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
+
@@ -21,7 +23,10 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
-
+
+
+
+
diff --git a/launch/klausen_dampen.launch b/launch/klausen_dampen.launch
index 8cdbe9c..24bcb44 100644
--- a/launch/klausen_dampen.launch
+++ b/launch/klausen_dampen.launch
@@ -31,7 +31,6 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
pkg="oscillation_ctrl"
type="ref_signalGen.py"
name="refSignal_node"
- launch-prefix="xterm -e"
/>
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/LinkState.py b/src/LinkState.py
index 6adfba3..c6c39b7 100755
--- a/src/LinkState.py
+++ b/src/LinkState.py
@@ -18,7 +18,7 @@ class Main:
def __init__(self):
# rate(s)
- rate = 40 # rate for the publisher method, specified in Hz -- 20 Hz
+ rate = 60 # rate for the publisher method, specified in Hz -- 20 Hz
self.dt = 1.0/rate
@@ -65,18 +65,9 @@ class Main:
# service(s)
self.service1 = '/gazebo/get_link_state'
- # need service list to check if models have spawned
-# self.service_list = rosservice.get_service_list()
-
# wait for service to exist
rospy.wait_for_service(self.service1,timeout=10)
-# 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.publisher = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1)
self.pub_wd = rospy.Publisher('/status/path_follow', Bool, queue_size=1)
@@ -229,6 +220,7 @@ class Main:
print "theta: " + str(round(self.theta*180/3.14,3))
# Populate message
+ self.status.header.stamp = rospy.Time.now()
self.status.drone_pos = drone_P.link_state.pose
self.status.pload_pos = pload_P.link_state.pose
self.status.length = self.tetherL
diff --git a/src/MoCap_Localization_noTether.py b/src/MoCap_Localization_noTether.py
index 1843342..a0fc3f0 100755
--- a/src/MoCap_Localization_noTether.py
+++ b/src/MoCap_Localization_noTether.py
@@ -17,7 +17,7 @@ class Main:
def __init__(self):
# rate(s)
- rate = 40 # rate for the publisher method, specified in Hz -- 20 Hz
+ rate = 100 # 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
diff --git a/src/MocapBridge.py b/src/MocapBridge.py
new file mode 100755
index 0000000..262a533
--- /dev/null
+++ b/src/MocapBridge.py
@@ -0,0 +1,195 @@
+#!/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 *
+import tf2_ros
+from oscillation_ctrl.msg import tethered_status
+from geometry_msgs.msg import PoseStamped, Point, TransformStamped
+from std_msgs.msg import Bool
+from tf2_msgs.msg import TFMessage
+
+class Main:
+
+ def __init__(self):
+
+ # rate(s)
+ rate = 50 # 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.desired_frames = ['LabDrone','payload'] # models of markers of interest
+
+ # Create necessary variables based on models of interest
+ for idx,name in enumerate(self.desired_frames):
+ exec('self.{:s}_pose = PoseStamped()'.format(name)) # ex) self.LabDrone_pose = PoseStamped()
+ exec('self.{:s}_pose.header.frame_id = "/map"'.format(name))
+
+ self.eul = [0.0,0.0,0.0]
+ self.has_run = 0
+ self.model_idx = 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('/tf', TFMessage, self.cortex_cb)
+
+ def cortex_cb(self,msg):
+
+ try:
+ # disect msg to get pose of each body
+ for i,data in enumerate(msg.transforms): # get length (i) and msg (data)
+ if data.child_frame_id in self.desired_frames:
+ #self.noname(data.child_frame_id,1) # send that string to execute desired action
+ exec('self.{:s}_pose.pose.position = data.transform.translation'.format(data.child_frame_id))
+ exec('self.{:s}_pose.pose.orientation = data.transform.rotation'.format(data.child_frame_id))
+ if self.model_idx < i: # check to see how many bodies tracked by mocap do we care about
+ self.model_idx = i
+
+ except ValueError:
+ pass
+
+ def noname(self,name,action):
+ """
+ Takes in string (name) and runs desired action
+ """
+ if action == 1:
+ exec('self.{:s}_pose.pose.position = data.transform.translation'.format(name))
+ exec('self.{:s}_pose.pose.orientation = data.transform.rotation'.format(name))
+ elif action == 2:
+ exec('self.pose_pub.publish(self.{:s}_pose)',)
+
+
+
+ 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,pose):
+ """
+ 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([pose.orientation.x,
+ pose.orientation.y,
+ pose.orientation.z,
+ pose.orientation.w])
+
+ def FRD_Transform(self):
+ '''
+ Transforms mocap reading to proper coordinate frame
+ '''
+
+ self.LabDrone_pose.header.stamp = rospy.Time.now()
+ self.euler_array(self.LabDrone_pose.pose) # get euler angles of orientation for user
+
+
+# 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 publisher(self,pub_timer):
+ self.FRD_Transform()
+ #self.fake_mocap() # used while debugging
+ #self.noname()
+ #if self.model_idx == 1:
+ self.pose_pub.publish(self.LabDrone_pose)
+ #elif self.model_idx == 2:
+ # self.pose_pub.publish(self.LabDrone_pose)
+ #self.pose_pub.publish(payload stuff)
+ print "\n"
+ print "drone pos.x: " + str(round(self.LabDrone_pose.pose.position.x,2))
+ print "drone pos.y: " + str(round(self.LabDrone_pose.pose.position.y,2))
+ print "drone pos.z: " + str(round(self.LabDrone_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))
+
+ def fake_mocap(self):
+ self.LabDrone_pose.header.stamp = rospy.Time.now()
+ self.LabDrone_pose.pose.position.x = 0.0
+ self.LabDrone_pose.pose.position.y = 0.0
+ self.LabDrone_pose.pose.position.z = 0.5
+ self.LabDrone_pose.pose.orientation.x = 0.0
+ self.LabDrone_pose.pose.orientation.y = 0.0
+ self.LabDrone_pose.pose.orientation.z = 0.0
+ self.LabDrone_pose.pose.orientation.w = 1.0
+ self.euler_array(self.LabDrone_pose.pose)
+
+
+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 cbdadfe..9a8eea2 100755
--- a/src/klausen_control.py
+++ b/src/klausen_control.py
@@ -36,6 +36,9 @@ class Main:
self.tmax = self.dt
self.n = self.tmax/self.dt + 1
self.t = np.linspace(0, self.tmax, self.n) # Time array
+ 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()
# Msgs types
self.vel_data = TwistStamped() # This is needed to get drone vel from gps
@@ -69,8 +72,12 @@ class Main:
if rospy.has_param('sim/tether_length'):
self.tetherL = rospy.get_param('sim/tether_length') # Tether length
self.param_exists = True
- self.tether = True # Assume we have a tether
-
+ self.tether = True
+ elif rospy.get_time() - self.tstart >= 10.0:
+ self.tetherL = 0.0
+ rospy.loginfo('waiting for tether length')
+ break
+
# Tuning gains
self.K1 = np.identity(3)
self.K2 = np.identity(5)
@@ -159,7 +166,7 @@ class Main:
try:
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 + 9.81]])
+ self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z + 9.81]]) #TODO
except ValueError:
pass
@@ -167,10 +174,11 @@ class Main:
# Check if vehicle has tether
def tether_check(self):
if self.tether == True:
- rospy.loginfo_once('USING TETHER MODEL')
+ rospy.loginfo('TETHER LENGTH:', self.tetherL)
else:
rospy.loginfo_once('NO TETHER DETECTED')
+
# ---------------------------------ODE SOLVER-------------------------------------#
def statespace(self,y,t,Ka,Kb,Kc):
# Need the statespace array:
diff --git a/src/path_follow.cpp b/src/path_follow.cpp
index 855a614..5f04a7a 100644
--- a/src/path_follow.cpp
+++ b/src/path_follow.cpp
@@ -30,7 +30,7 @@ mavros_msgs::State current_state;
geometry_msgs::PoseStamped desPose;
tf2::Quaternion q_cmd /*What I send*/, q_Jae; // What should be sent;
-double rol_K, pch_K, yaw_K, rol_J, pch_J, yaw_J; // Same logic, K = cmd
+double rol_K, pch_K, yaw_K, rol_J, pch_J, yaw_J; // Same logic, K = Klausen
geometry_msgs::Vector3 K_ang, J_ang; // Needed to populate EulerAngle msg
oscillation_ctrl::EulerAngles eulAng;
@@ -229,23 +229,19 @@ int main(int argc, char **argv)
attitude.thrust = thrust.thrust;
// Determine which messages to send
- if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff){
+ if(ros::Time::now() - tkoff_req > ros::Duration(22.0) && takeoff){
attitude.orientation = q_des;
attitude.header.stamp = ros::Time::now();
att_targ.publish(attitude);
- att_cmds = true;
+ att_cmds = true;
+ ROS_INFO("Attitude Ctrl");
+ ROS_INFO("Des Altitude: %.2f", alt_des);
+ ROS_INFO("Cur Altitude: %.2f", current_altitude);
} else {
local_pos_pub.publish(buff_pose);
- }
-
- if(!att_cmds && ros::Time::now() - last_request > ros::Duration(2.0)){
- ROS_INFO("Thro: %.2f", attitude.thrust);
- } else {
- ROS_INFO("Thro: %.2f", attitude.thrust);
+ ROS_INFO("Position Ctrl");
ROS_INFO("Des Altitude: %.2f", alt_des);
- ROS_INFO("Cur Altitude: %.2f", current_altitude);
- //ROS_INFO("Sending points:\nRoll = %.2f\nPitch = %.2f",
- // 180*rol_K/3.141,180*pch_K/3.141);
+ ROS_INFO("Cur Altitude: %.2f", current_altitude);
}
// Publish euler angs
diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py
index e344399..9fe107a 100755
--- a/src/ref_signalGen.py
+++ b/src/ref_signalGen.py
@@ -29,8 +29,7 @@ class Main:
# 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()
- rospy.loginfo(self.tstart)
+ self.tstart = rospy.get_time()
self.dt = 1.0/rate
# self.dt = 0.5
@@ -67,7 +66,7 @@ class Main:
if rospy.has_param('sim/tether_length'):
self.tetherL = rospy.get_param('sim/tether_length') # Tether length
self.param_exists = True
- elif rospy.get_time() - self.tstart >= 3.0:
+ elif rospy.get_time() - self.tstart >= 10.0:
self.tetherL = 0.0
break
@@ -371,15 +370,12 @@ class Main:
# Populate msg with epsilon_final
self.ref_sig.header.stamp = rospy.Time.now()
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] + 0.5
-
+ 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]
self.ref_sig.acceleration.z = self.EPS_F[8]
@@ -426,7 +422,6 @@ class Main:
self.pub_path.publish(self.ref_sig)
self.pub_ref.publish(self.ref_sig)
-
# Give user feedback on published message:
self.screen_output()
diff --git a/src/wpoint_tracker.py b/src/wpoint_tracker.py
index 831edf1..150c934 100755
--- a/src/wpoint_tracker.py
+++ b/src/wpoint_tracker.py
@@ -46,7 +46,6 @@ class Main:
# publisher(s)
-
def waypoint_relay(self,req):
#TODO Need to add check to make sure query is boolean
if req.query:
@@ -54,7 +53,7 @@ class Main:
self.resp.xd = self.xd
else:
self.resp.xd = self.xd
- return self.resp
+ return self.resp
# Change desired position if there is a change
def waypoints_cb(self,msg):