diff --git a/.gitignore b/.gitignore index e14a7e0..6e83146 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,9 @@ launch/debug.launch launch/cortex_bridge.launch src/MoCap_Localization_*.py -src/MocapBridge.py +src/Mocap_*.py src/*_client.py +msg/Marker.msg +msg/Markers.msg *.rviz + diff --git a/launch/cortex_bridge.launch b/launch/cortex_bridge.launch index de74779..c3896a6 100644 --- a/launch/cortex_bridge.launch +++ b/launch/cortex_bridge.launch @@ -14,7 +14,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo diff --git a/msg/LoadAngles.msg b/msg/LoadAngles.msg new file mode 100644 index 0000000..2b2ddde --- /dev/null +++ b/msg/LoadAngles.msg @@ -0,0 +1,6 @@ +# Reports position of drone, payload, and the roll angle between them +std_msgs/Header header # Header +float32 phi # Roll angle between drone and pload +float32 phidot # Roll rate +float32 theta # Pitch angle between drone and pload +float32 thetadot # Pitch rate diff --git a/msg/tethered_status.msg b/msg/tethered_status.msg index 2d50d4b..0a05e8c 100644 --- a/msg/tethered_status.msg +++ b/msg/tethered_status.msg @@ -1,12 +1,6 @@ # Reports position of drone, payload, and the roll angle between them std_msgs/Header header # Header -string drone_id # Drone ID +std_msgs/Bool bool # Boolean, True = Payload geometry_msgs/Pose drone_pos # Drone pose -# sensor_msgs/Imu drone_acc # Drone linear acceleration -string pload_id # Payload ID geometry_msgs/Pose pload_pos # Payload pose -float32 length # Tether length -float32 phi # Roll angle between drone and pload -float32 phidot # Roll rate -float32 theta # Pitch angle between drone and pload -float32 thetadot # Pitch rate + diff --git a/src/MocapBridge.py b/src/MocapBridge.py deleted file mode 100755 index 39136eb..0000000 --- a/src/MocapBridge.py +++ /dev/null @@ -1,208 +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 * -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 = 30 # 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]) - -# 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.LabDrone_pose.pose.orientation.x = self.q[0] -# self.LabDrone_pose.pose.orientation.y = self.q[1] -# self.LabDrone_pose.pose.orientation.z = self.q[2] - - 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.header.stamp.secs = self.LabDrone_pose.header.stamp.secs - self.tstart - 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/offb_node.cpp b/src/offb_node.cpp index 2dd0082..28e69bc 100644 --- a/src/offb_node.cpp +++ b/src/offb_node.cpp @@ -114,7 +114,7 @@ int main(int argc, char **argv) // Populate pose msg pose.pose.position.x = 0; pose.pose.position.y = 0; - pose.pose.position.z = 2.5; + pose.pose.position.z = 1.5; pose.pose.orientation.x = q_init.x; pose.pose.orientation.y = q_init.y; pose.pose.orientation.z = q_init.z;