From 30fd69b4ec4bfc3141ef9c27e021b371486583a5 Mon Sep 17 00:00:00 2001 From: cesar Date: Fri, 4 Mar 2022 12:23:52 -0400 Subject: [PATCH] Added gitignore --- .gitignore | 3 + package.xml | 5 +- src/LinkState.py | 0 ...zation.py => MoCap_Localization_Tether.py} | 0 src/MoCap_Localization_noTether.py | 153 ++++++++++++++++++ src/klausen_control.py | 0 src/ref_signalGen.py | 2 +- src/square.py | 0 src/step.py | 0 9 files changed, 158 insertions(+), 5 deletions(-) create mode 100644 .gitignore mode change 100644 => 100755 src/LinkState.py rename src/{MoCap_Localization.py => MoCap_Localization_Tether.py} (100%) mode change 100644 => 100755 create mode 100755 src/MoCap_Localization_noTether.py mode change 100644 => 100755 src/klausen_control.py mode change 100644 => 100755 src/ref_signalGen.py mode change 100644 => 100755 src/square.py mode change 100644 => 100755 src/step.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..2a5e834 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +models +citadel_hill_world.world + diff --git a/package.xml b/package.xml index ad34a0b..9b73b92 100644 --- a/package.xml +++ b/package.xml @@ -25,15 +25,12 @@ geometry_msgs std_msgs - stds_msgs + std_msgs message_generation message_runtime message_runtime - px4 - px4 - diff --git a/src/LinkState.py b/src/LinkState.py old mode 100644 new mode 100755 diff --git a/src/MoCap_Localization.py b/src/MoCap_Localization_Tether.py old mode 100644 new mode 100755 similarity index 100% rename from src/MoCap_Localization.py rename to src/MoCap_Localization_Tether.py diff --git a/src/MoCap_Localization_noTether.py b/src/MoCap_Localization_noTether.py new file mode 100755 index 0000000..56e541a --- /dev/null +++ b/src/MoCap_Localization_noTether.py @@ -0,0 +1,153 @@ +#!/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 +from std_msgs.msg import Bool + +class Main: + + def __init__(self): + + # rate(s) + rate = 40 # rate for the publisher method, specified in Hz -- 20 Hz + + self.dt = 1.0/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() + + ### -*-*-*- 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() + + # 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) + #self.publisher = rospy.Publisher('/status/twoBody_status', tethered_status, 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.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,pose): + """ + Takes in pose msg object and outputs array of euler angs: + q[0] = Roll + q[1] = Pitch + q[2] = Yaw + """ + q = euler_from_quaternion([pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w]) + return q + +# def FRD_Transform(pose) +# ''' +# Transforms mocap reading to proper coordinate frame +# ''' +# pose.position.x = +# pose.position.y = +# pose.position.z = + +# Keep the w same and change x, y, and z as above. +# pose.orientation.x = +# pose.orientation.y = +# pose.orientation.z = +# pose.orientation.w = + +# return pose + + +# 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.pose_pub.publish(self.drone_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 old mode 100644 new mode 100755 diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py old mode 100644 new mode 100755 index 118cf07..2fa367d --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -11,7 +11,7 @@ import math from scipy import signal from scipy.integrate import odeint -from offboard_ex.msg import tethered_status, RefSignal +from oscillation_ctrl.msg import tethered_status, RefSignal from controller_msgs.msg import FlatTarget diff --git a/src/square.py b/src/square.py old mode 100644 new mode 100755 diff --git a/src/step.py b/src/step.py old mode 100644 new mode 100755