forked from cesar.alejandro/oscillation_ctrl
Weekly improvements
This commit is contained in:
parent
798da36dc1
commit
527f5cb9e6
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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_*'
|
||||
|
|
|
|||
|
|
@ -9,10 +9,12 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
<arg name="fcu_protocol" default="v2.0" />
|
||||
<arg name="respawn_mavros" default="false" />
|
||||
<arg name="gazebo_gui" default="false" />
|
||||
<!--arg name="fcu_url" default="udp://:14551@192.168.1.91:14556"/>
|
||||
<arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" /-->
|
||||
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="MoCap_Localization_noTether.py"
|
||||
type="MocapBridge.py"
|
||||
name="localize_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
|
|
@ -21,7 +23,10 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
<include file="$(find cortex_bridge)/launch/cortex_bridge.launch"/>
|
||||
|
||||
<!-- MAVROS launch -->
|
||||
<include file="$(find mavros)/launch/px4.launch"/>
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="pluginlists_yaml" value="$(find oscillation_ctrl)/config/px4_pluginlists.yaml" />
|
||||
<arg name="config_yaml" value="$(find oscillation_ctrl)/config/px4_config.yaml" />
|
||||
<arg name="fcu_url" value="udp://:14549@192.168.1.91:14554" />
|
||||
<arg name="gcs_url" value="udp-b://127.0.0.1:14555@14550" />
|
||||
</include>
|
||||
</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"
|
||||
/>
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
|
|
|
|||
|
|
@ -0,0 +1,23 @@
|
|||
<launch>
|
||||
<!-- vim: set ft=xml noet : -->
|
||||
<!-- example launch script for PX4 based FCU's -->
|
||||
<arg name="fcu_url" default="udp://:14551@192.168.1.91:14556"/>
|
||||
<arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" />
|
||||
<arg name="tgt_system" default="1" />
|
||||
<arg name="tgt_component" default="1" />
|
||||
<arg name="log_output" default="screen" />
|
||||
<arg name="fcu_protocol" default="v2.0" />
|
||||
<arg name="respawn_mavros" default="false" />
|
||||
|
||||
<include file="$(find mavros)/launch/node.launch">
|
||||
<arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
|
||||
<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
|
||||
<arg name="fcu_url" value="$(arg fcu_url)" />
|
||||
<arg name="gcs_url" value="$(arg gcs_url)" />
|
||||
<arg name="tgt_system" value="$(arg tgt_system)" />
|
||||
<arg name="tgt_component" value="$(arg tgt_component)" />
|
||||
<arg name="log_output" value="$(arg log_output)" />
|
||||
<arg name="fcu_protocol" value="$(arg fcu_protocol)" />
|
||||
<arg name="respawn_mavros" default="$(arg respawn_mavros)" />
|
||||
</include>
|
||||
</launch>
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
Loading…
Reference in New Issue