cesar.alejandro
/
oscillation_ctrl
1
0
Fork 1
Code Issues Pull Requests Projects Releases Wiki Activity

Updates

This commit is contained in:
cesar 2022-04-02 16:21:20 -03:00
parent 792b791ef8
commit 6acf710357
6 changed files with 14 additions and 219 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored
View File

@ -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

2
launch/cortex_bridge.launch
View File

@ -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"
/>

6
msg/LoadAngles.msg Normal file
View File

@ -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

10
msg/tethered_status.msg
View File

@ -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

208
src/MocapBridge.py
View File

@ -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

2
src/offb_node.cpp
View File

@ -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;