forked from cesar.alejandro/oscillation_ctrl
Updates
This commit is contained in:
parent
792b791ef8
commit
6acf710357
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="MocapBridge.py"
|
||||
type="Mocap_Bridge.py"
|
||||
name="localize_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue