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

Lastest changes to algorithm + adding airframes to work with Ubuntu 20.04

This commit is contained in:
cesar 2022-09-14 14:19:12 -03:00
parent 5b224e599d
commit 937fd75fee
28 changed files with 436 additions and 1070 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored
View File

@ -2,10 +2,13 @@ config/mocap_*
launch/cortex_bridge.launch launch/cortex_bridge.launch
launch/debug.launch launch/debug.launch
launch/klausen_dampen.launch launch/klausen_dampen.launch
launch/mocap_*
src/development_* src/development_*
src/killswitch_client.py src/killswitch_client.py
src/land_client.py src/land_client.py
src/MoCap_*.py
src/Mocap_*.py src/Mocap_*.py
src/mocap_*
src/segmented_tether.py src/segmented_tether.py
src/segmented_tether_fast.py src/segmented_tether_fast.py
msg/Marker.msg msg/Marker.msg

6
CMakeLists.txt
View File

@ -95,11 +95,11 @@ add_dependencies(pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPO
#add_dependencies(mocap_offb_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS}) #add_dependencies(mocap_offb_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(mocap_pathFollow_node src/mocap_path_follow.cpp) #add_executable(mocap_pathFollow_node src/mocap_path_follow.cpp)
target_link_libraries(mocap_pathFollow_node ${catkin_LIBRARIES}) #target_link_libraries(mocap_pathFollow_node ${catkin_LIBRARIES})
add_dependencies(mocap_pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS}) #add_dependencies(mocap_pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ library ## Declare a C++ library
# add_library(${PROJECT_NAME} # add_library(${PROJECT_NAME}

25
airframes/18.04/4000_spiri Normal file
View File

@ -0,0 +1,25 @@
#!/bin/sh
#
# @name Generic Quadcopter
#
# @type Quadrotor x
# @class Copter
. ${R}etc/init.d/rc.mc_defaults
param set-default MC_ROLLRATE_K 2.35
param set-default MC_ROLLRATE_D 0.0032
param set-default MC_ROLLRATE_I 0.15
param set-default MC_PITCHRATE_K 2.35
param set-default MC_PITCHRATE_D 0.0032
param set-default MC_PITCHRATE_I 0.15
param set-default MPC_Z_VEL_MAX_UP 1.0
param set EKF2_AID_MASK 1
param set EKF2_HGT_MODE 0
param set SYS_FAILURE_EN 0
set MIXER quad_x
set PWM_OUT 1234

26
airframes/18.04/4001_spiri_with_tether Normal file
View File

@ -0,0 +1,26 @@
#!/bin/sh
#
# @name Generic Quadcopter
#
# @type Quadrotor x
# @class Copter
. ${R}etc/init.d/rc.mc_defaults
param set MC_ROLLRATE_K 2.35
param set MC_ROLLRATE_D 0.0030
param set MC_ROLLRATE_I 0.15
param set MC_PITCHRATE_K 2.35
param set MC_PITCHRATE_D 0.0030
param set MC_PITCHRATE_I 0.15
param set MPC_Z_VEL_MAX_UP 0.5
param set MPC_TKO_SPEED 1.0
param set EKF2_AID_MASK 1
param set EKF2_HGT_MODE 0
param set SYS_FAILURE_EN 0
set MIXER quad_x
set PWM_OUT 1234

0
airframes/4000_spiri → airframes/20.04/4000_spiri
View File

8
airframes/4001_spiri_with_tether → airframes/20.04/4001_spiri_with_tether
View File

@ -16,11 +16,6 @@ param set-default MC_PITCHRATE_D 0.0060
param set-default MC_PITCHRATE_I 0.35 param set-default MC_PITCHRATE_I 0.35
param set-default MPC_Z_P 0.70 param set-default MPC_Z_P 0.70
#param load spiri_param/Vehicle_230_Parameters.params
#param set-default MC_PITCHRATE_P 0.0889
#param set-default MC_ROLLRATE_P 0.0957
param set-default CA_AIRFRAME 0 param set-default CA_AIRFRAME 0
param set-default CA_ROTOR_COUNT 4 param set-default CA_ROTOR_COUNT 4
@ -45,8 +40,7 @@ param set-default COM_RCL_EXCEPT 4
param set-default MPC_Z_VEL_MAX_UP 1.0 param set-default MPC_Z_VEL_MAX_UP 1.0
param set-default COM_RCL_EXCEPT 4 param set-default COM_RCL_EXCEPT 4 # Ignores no RC failsafe (not needed for simulations)
param set-default NAV_RCL_ACT 1
set MIXER quad_x set MIXER quad_x
set PWM_OUT 1234 set PWM_OUT 1234

14
config/Ctrl_param.yaml
View File

@ -1,14 +0,0 @@
# Ros param when using Klausen Ctrl
wait_time: 30
#drone_mass: 0.614 # weight with new battery
drone_mass: 0.602 # weight with old battery
#drone_mass: 1.437 # spiri weight
#pload_mass: 0.15 # Pload mass with 100g weight
pload_mass: 0.10 # Pload mass with 50g weight
#pload_mass: 0.05 # Pload mass with just basket
#pload_mass: 0.25
use_ctrl: false

6
config/gazebo_config.yaml
View File

@ -1,6 +0,0 @@
# Ros param when not using Klausen Ctrl
waypoints: {x: 0.0, y: -0.25, z: 1.5}
square_x: [0.5,1,1,1,0.5,0,0]
square_y: [0,0,0.5,1,1,1,0.5]
hover_throttle: 0.46 # with 500g
hover_throttle: 0.51 # with 250g???

14
config/mocapGazebo_params.yaml Normal file
View File

@ -0,0 +1,14 @@
# Ros param when using Klausen Ctrl
wait_time: 30 # parameter which can be set to run desired tests at a desired time
#drone_mass: 0.614 # weight with new battery
drone_mass: 0.602
pload_mass: 0.1 # mass of payload. Needs to be changed in spiri_with_tether file as well
#pload_mass: 0.15 # Pload mass with 100g weight
pload_mass: 0.10 # Pload mass with 50g weight
#pload_mass: 0.05 # Pload mass with just basket
#pload_mass: 0.25
use_ctrl: false # starts PX4 without attitude controller
waypoints: {x: 0.0, y: 0.0, z: 5.0} # takeoff waypoints
# sets waypoints to run a square test
square_x: [0.5,1,1,1,0.5,0,0]
square_y: [0,0,0.5,1,1,1,0.5]

27
config/mocapLab_params.yaml Normal file
View File

@ -0,0 +1,27 @@
# Ros param when using Klausen Ctrl
wait_time: 30 # parameter which can be set to run desired tests at a desired time
# DRONE MASSES
#drone_mass: 0.614 # weight with new battery
drone_mass: 0.602
#PLOAD MASSES
#pload_mass: 0.15 # Pload mass with 100g weight
pload_mass: 0.10 # Pload mass with 50g weight
#pload_mass: 0.05 # Pload mass with just basket
#pload_mass: 0.25
# CTRL PARAMETER - should be false to start always
use_ctrl: false # starts PX4 without attitude controller
waypoints: {x: 0.0, y: 0.0, z: 1.75} # takeoff waypoints
# sets waypoints to run a square test
square_x: [0.5,1,1,1,0.5,0,0]
square_y: [0,0,0.5,1,1,1,0.5]
# HOVER THROTTLE - Changes depending on mass of pload and drone
# hover_throttle: 0.32 # Hover throttle with pload 0.15 kg
hover_throttle: 0.28 # Hover throttle with pload 0.10 kg
# hover_throttle: 0.22 # Hover throttle with no pload
#hover_throttle: 0.26 # Hover throttle with pload 0.05 kg

7
config/noCtrl_param.yaml
View File

@ -1,7 +0,0 @@
# Ros param when not using Klausen Ctrl
wait_time: 40
drone_mass: 0.5841
#drone_mass: 1.437
pload_mass: 0.10

9
config/spiri_params.yaml Normal file
View File

@ -0,0 +1,9 @@
# Set useful ROS parameters for simulation
wait_time: 30 # parameter which can be set to run desired tests at a desired time
drone_mass: 1.437 # mass of drone
pload_mass: 0.25 # mass of payload
use_ctrl: false # starts PX4 without attitude controller - needs to be set to false to takeoff
waypoints: {x: 0.0, y: 0.0, z: 5.0} # takeoff waypoints
# sets waypoints to run a square test
square_x: [0.5,1,1,1,0.5,0,0]
square_y: [0,0,0.5,1,1,1,0.5]

54
launch/headless_spiri_mocap.launch Normal file
View File

@ -0,0 +1,54 @@
<?xml version="1.0"?>
<launch>
<!-- MAVROS posix SITL environment launch script -->
<!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
<!-- vehicle pose -->
<arg name="x" default="0"/>
<arg name="y" default="0"/>
<arg name="z" default="0"/>
<arg name="R" default="0"/>
<arg name="P" default="0"/>
<arg name="Y" default="0"/>
<!-- vehicle model and world -->
<arg name="est" default="ekf2"/>
<arg name="vehicle" default="spiri_mocap"/>
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/citadel_hill_world.world"/>
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
<!-- gazebo configs -->
<arg name="gui" default="false"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>
<arg name="respawn_gazebo" default="false"/>
<!-- MAVROS configs -->
<arg name="fcu_url" default="udp://:14540@localhost:14557"/>
<arg name="respawn_mavros" default="false"/>
<!-- PX4 configs -->
<arg name="interactive" default="true"/>
<!-- PX4 SITL and Gazebo -->
<include file="$(find px4)/launch/posix_sitl.launch">
<arg name="x" value="$(arg x)"/>
<arg name="y" value="$(arg y)"/>
<arg name="z" value="$(arg z)"/>
<arg name="R" value="$(arg R)"/>
<arg name="P" value="$(arg P)"/>
<arg name="Y" value="$(arg Y)"/>
<arg name="world" value="$(arg world)"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="sdf" value="$(arg sdf)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="interactive" value="$(arg interactive)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<!-- GCS link is provided by SITL -->
<arg name="gcs_url" value=""/>
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
</include>
</launch>

54
launch/headless_spiri_with_tether_mocap.launch Normal file
View File

@ -0,0 +1,54 @@
<?xml version="1.0"?>
<launch>
<!-- MAVROS posix SITL environment launch script -->
<!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
<!-- vehicle pose -->
<arg name="x" default="0"/>
<arg name="y" default="0"/>
<arg name="z" default="0"/>
<arg name="R" default="0"/>
<arg name="P" default="0"/>
<arg name="Y" default="0"/>
<!-- vehicle model and world -->
<arg name="est" default="ekf2"/>
<arg name="vehicle" default="spiri_with_tether"/>
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/citadel_hill_world.world"/>
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
<!-- gazebo configs -->
<arg name="gui" default="false"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>
<arg name="respawn_gazebo" default="false"/>
<!-- MAVROS configs -->
<arg name="fcu_url" default="udp://:14540@localhost:14557"/>
<arg name="respawn_mavros" default="false"/>
<!-- PX4 configs -->
<arg name="interactive" default="true"/>
<!-- PX4 SITL and Gazebo -->
<include file="$(find px4)/launch/posix_sitl.launch">
<arg name="x" value="$(arg x)"/>
<arg name="y" value="$(arg y)"/>
<arg name="z" value="$(arg z)"/>
<arg name="R" value="$(arg R)"/>
<arg name="P" value="$(arg P)"/>
<arg name="Y" value="$(arg Y)"/>
<arg name="world" value="$(arg world)"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="sdf" value="$(arg sdf)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="interactive" value="$(arg interactive)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
</include>
<!-- MAVROS -->
<include file="$(find oscillation_ctrl)/launch/px4.launch">
<!-- GCS link is provided by SITL -->
<arg name="gcs_url" value=""/>
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
</include>
</launch>

7
launch/mocap_oscillation_damp.launch
View File

@ -14,13 +14,8 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
<arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" /> <arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" />
<arg name="connection_type" default="wifi"/> <arg name="connection_type" default="wifi"/>
<group ns="mocap">
<rosparam file="$(find oscillation_ctrl)/config/mocap_config.yaml" />
</group>
<group ns="status"> <group ns="status">
<rosparam file="$(find oscillation_ctrl)/config/Ctrl_param.yaml" /> <rosparam file="$(find oscillation_ctrl)/config/mocapLab_params.yaml" />
<param name="test_type" value="$(arg test)"/> <param name="test_type" value="$(arg test)"/>
</group> </group>

35
launch/mocap_sim.launch
View File

@ -3,12 +3,10 @@
Launch file to use klausen oscillaton damping ctrl in Gazebo Launch file to use klausen oscillaton damping ctrl in Gazebo
/--> /-->
<launch> <launch>
<arg name="model" default="headless_spiri_mocap"/> <group ns="status">
<arg name="ctrl" default="yes"/> <rosparam file="$(find oscillation_ctrl)/config/mocapGazebo_params.yaml" />
<group ns="sim"> <!--> should be mocap but will use gazebo since it is still sim <-->
<rosparam file="$(find oscillation_ctrl)/config/gazebo_config.yaml"/>
</group> </group>
<arg name="model" default="headless_spiri"/>
<node <node
pkg="oscillation_ctrl" pkg="oscillation_ctrl"
type="MoCap_Localization_fake.py" type="MoCap_Localization_fake.py"
@ -21,22 +19,6 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
type="wpoint_tracker.py" type="wpoint_tracker.py"
name="waypoints_server" name="waypoints_server"
/> />
<group if="$(eval ctrl == 'no')">
<group ns="status">
<rosparam file="$(find oscillation_ctrl)/config/noCtrl_param.yaml" />
</group>
<node
pkg="oscillation_ctrl"
type="offb_node"
name="offb_node"
launch-prefix="xterm -e"
/>
</group>
<!-- RUNS WITH CRTL -->
<group if="$(eval ctrl =='yes')">
<group ns="status">
<rosparam file="$(find oscillation_ctrl)/config/Ctrl_param.yaml" />
</group>
<!-- CREATES DESIRED TRAJECTORY/ REFERENCE SIGNAL --> <!-- CREATES DESIRED TRAJECTORY/ REFERENCE SIGNAL -->
<node <node
pkg="oscillation_ctrl" pkg="oscillation_ctrl"
@ -64,8 +46,15 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
name="mocap_Test" name="mocap_Test"
launch-prefix="xterm -e" launch-prefix="xterm -e"
/> />
</group> <!-- SETS PLOAD MASS -->
<node
pkg="oscillation_ctrl"
type="set_ploadmass.py"
name="set_ploadmass"
output="screen"
/>
<!-- PX4 LAUNCH --> <!-- PX4 LAUNCH -->
<include file="$(find px4)/launch/$(arg model).launch"/> <include file="$(find oscillation_ctrl)/launch/$(arg model)_mocap.launch">
</include>
</launch> </launch>

65
launch/mocap_takeoff_noCtrl.launch
View File

@ -1,65 +0,0 @@
<?xml version="1.0"?>
<launch>
<arg name='test' default="none"/>
<arg name="mav_name" default="spiri"/>
<arg name="command_input" default="1" />
<arg name="log_output" default="screen" />
<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://:14549@192.168.1.91:14554"/>
<arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" />
<arg name="connection_type" default="wifi"/>
<group ns="mocap">
<rosparam file="$(find oscillation_ctrl)/config/mocap_config.yaml" />
</group>
<group ns="status">
<rosparam file="$(find oscillation_ctrl)/config/noCtrl_param.yaml" />
<param name="test_type" value="$(arg test)"/>
</group>
<group if="$(eval connection_type == 'ethernet')">
<param name="local_ip" value="192.168.1.175"/>
</group>
<group if="$(eval connection_type == 'wifi')">
<param name="local_ip" value="192.168.1.135"/>
</group>
<node
pkg="oscillation_ctrl"
type="mocap_offb_node"
name="mocap_offb_node"
launch-prefix="xterm -e"
/>
<node
pkg="oscillation_ctrl"
type="wpoint_tracker.py"
name="waypoints_server"
/>
<group if="$(eval test != 'none')">
<node
pkg="oscillation_ctrl"
type="perform_test.py"
name="test_node"
launch-prefix="xterm -e"
/>
</group>
<node
pkg="oscillation_ctrl"
type="Mocap_Bridge.py"
name="localize_node"
launch-prefix="xterm -e"
/>
<!-- Cortex bridge launch -->
<include file="$(find cortex_bridge)/launch/cortex_bridge.launch">
<!--param name="local_ip" value="$(param local_ip)" /-->
</include>
<!-- MAVROS 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_protocol" value="$(arg fcu_protocol)" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
</include>
</launch>

7
launch/oscillation_damp.launch
View File

@ -5,11 +5,9 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
<launch> <launch>
<arg name="model" default="headless_spiri_with_tether"/> <arg name="model" default="headless_spiri_with_tether"/>
<arg name="test" default="none"/> <arg name="test" default="none"/>
<group ns="sim">
<rosparam file="$(find oscillation_ctrl)/config/gazebo_config.yaml" />
</group>
<group ns="status"> <group ns="status">
<rosparam file="$(find oscillation_ctrl)/config/Ctrl_param.yaml" /> <rosparam file="$(find oscillation_ctrl)/config/spiri_params.yaml" />
<param name="test_type" value="$(arg test)"/> <param name="test_type" value="$(arg test)"/>
</group> </group>
@ -37,7 +35,6 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
pkg="oscillation_ctrl" pkg="oscillation_ctrl"
type="klausen_control.py" type="klausen_control.py"
name="klausenCtrl_node" name="klausenCtrl_node"
launch-prefix="xterm -e"
/> />
<!-- PUBLISHES DESIRED COMMANDS --> <!-- PUBLISHES DESIRED COMMANDS -->
<node <node

47
src/LinkState.py
View File

@ -9,7 +9,6 @@ import time
import math import math
from tf.transformations import * from tf.transformations import *
from oscillation_ctrl.msg import TetheredStatus, LoadAngles from oscillation_ctrl.msg import TetheredStatus, LoadAngles
from geometry_msgs.msg import Pose
from gazebo_msgs.srv import GetLinkState from gazebo_msgs.srv import GetLinkState
from std_msgs.msg import Bool from std_msgs.msg import Bool
@ -53,10 +52,6 @@ class Main:
self.thetabuf = 0.0 # self.thetabuf = 0.0 #
self.pload = True # Check if payload exists self.pload = True # Check if payload exists
# Max dot values to prevent 'blowup'
self.phidot_max = 3.0
self.thetadot_max = 3.0
# service(s) # service(s)
self.service1 = '/gazebo/get_link_state' self.service1 = '/gazebo/get_link_state'
@ -72,25 +67,6 @@ class Main:
self.path_timer = rospy.Timer(rospy.Duration(40.0/rate), self.path_follow) self.path_timer = rospy.Timer(rospy.Duration(40.0/rate), self.path_follow)
self.gui_timer = rospy.Timer(rospy.Duration(1/10.0), self.user_feedback) self.gui_timer = rospy.Timer(rospy.Duration(1/10.0), self.user_feedback)
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,orientation): def euler_array(self,orientation):
""" """
Takes in pose msg object and outputs array of euler angs: Takes in pose msg object and outputs array of euler angs:
@ -171,7 +147,6 @@ class Main:
# Determine thetadot # Determine thetadot
self.loadAngles.thetadot = (self.loadAngles.theta - self.thetabuf)/self.dt self.loadAngles.thetadot = (self.loadAngles.theta - self.thetabuf)/self.dt
self.loadAngles.thetadot = self.cutoff(self.loadAngles.thetadot,self.thetadot_max)
self.thetabuf = self.loadAngles.theta self.thetabuf = self.loadAngles.theta
# Determine phi (roll) # Determine phi (roll)
@ -184,7 +159,6 @@ class Main:
# Determine phidot # Determine phidot
self.loadAngles.phidot = (self.loadAngles.phi - self.phibuf)/self.dt self.loadAngles.phidot = (self.loadAngles.phi - self.phibuf)/self.dt
self.loadAngles.phidot = self.cutoff(self.loadAngles.phidot,self.phidot_max)
self.phibuf = self.loadAngles.phi # Update buffer self.phibuf = self.loadAngles.phi # Update buffer
else: # Otherwise, vars = 0 else: # Otherwise, vars = 0
@ -204,17 +178,18 @@ class Main:
rospy.loginfo("Get Link State call failed: {0}".format(e)) rospy.loginfo("Get Link State call failed: {0}".format(e))
def user_feedback(self,gui_timer): def user_feedback(self,gui_timer):
# Print and save results print ("\n")
#print "\n"
rospy.loginfo("") rospy.loginfo("")
print"Roll: "+str(round(self.drone_Eul[0]*180/3.14,2)) print("Drone pos.x: " + str(round(self.drone_Px,2)))
print"Pitch: " +str(round(self.drone_Eul[1]*180/3.14,2)) print("Drone pos.y: " + str(round(self.drone_Py,2)))
print"Yaw: " +str(round(self.drone_Eul[2]*180/3.14,2)) print("Drone pos.z: " + str(round(self.drone_Pz,2)))
print "drone pos.x: " + str(round(self.drone_Px,2)) print("Roll: "+str(round(self.drone_Eul[0]*180/3.14,2)))
print "drone pos.y: " + str(round(self.drone_Py,2)) print("Pitch: " +str(round(self.drone_Eul[1]*180/3.14,2)))
print "drone pos.z: " + str(round(self.drone_Pz,2)) print("Yaw: " +str(round(self.drone_Eul[2]*180/3.14,2)))
print "phi: " + str(round(self.loadAngles.phi*180/3.14,3)) if self.pload:
print "theta: " + str(round(self.loadAngles.theta*180/3.14,3)) print("Tether length: " + str(round(self.tetherL,2)))
print("Theta: " + str(round(self.loadAngles.theta*180/3.14,2)))
print("Phi: " + str(round(self.loadAngles.phi*180/3.14,2)))
def path_follow(self,path_timer): def path_follow(self,path_timer):
now = rospy.get_time() now = rospy.get_time()

183
src/MoCap_Localization_noTether.py
View File

@ -1,183 +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 *
from oscillation_ctrl.msg import tethered_status
from geometry_msgs.msg import PoseStamped, Point
from std_msgs.msg import Bool
class Main:
def __init__(self):
# rate(s)
rate = 120 # 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.eul = [0.0,0.0,0.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('/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):
"""
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([self.drone_pose.pose.orientation.x,
self.drone_pose.pose.orientation.y,
self.drone_pose.pose.orientation.z,
self.drone_pose.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.drone_pose.pose.orientation.x = self.q[0]
self.drone_pose.pose.orientation.y = self.q[1]
self.drone_pose.pose.orientation.z = self.q[2]
self.drone_pose.pose.orientation.w = self.q[3]
def FRD_Transform(self):
'''
Transforms mocap reading to proper coordinate frame
'''
# self.drone_pose = self.buff_pose
self.drone_pose.header.frame_id = "/map"
# self.drone_pose.pose.position.x = 0
# self.drone_pose.pose.position.y = 0
# self.drone_pose.pose.position.z = 0.5
#Keep the w same and change x, y, and z as above.
# self.drone_pose.pose.orientation.x = 0
# self.drone_pose.pose.orientation.y = 0
# self.drone_pose.pose.orientation.z = 0
# self.drone_pose.pose.orientation.w = 1
self.euler_array() # get euler angles of orientation for user
# self.drone_pose.pose.position.x = self.buff_pose.pose.position.y
# self.drone_pose.pose.position.y = self.buff_pose.pose.position.x
# self.drone_pose.pose.position.z = -self.buff_pose.pose.position.z
# Keep the w same and change x, y, and z as above.
# self.drone_pose.pose.orientation.x = self.buff_pose.pose.orientation.y
# self.drone_pose.pose.orientation.y = self.buff_pose.pose.orientation.x
# self.drone_pose.pose.orientation.z = -self.buff_pose.pose.orientation.z
# self.drone_pose.pose.orientation.w = self.buff_pose.pose.orientation.w
# 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.FRD_Transform()
self.pose_pub.publish(self.drone_pose)
print "\n"
rospy.loginfo("")
print "drone pos.x: " + str(round(self.drone_pose.pose.position.x,2))
print "drone pos.y: " + str(round(self.drone_pose.pose.position.y,2))
print "drone pos.z: " + str(round(self.drone_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))
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

60
src/klausen_control.py
View File

@ -1,4 +1,4 @@
#!/usr/bin/env python2.7 #!/usr/bin/env python
### Cesar Rodriguez Aug 2021 ### Cesar Rodriguez Aug 2021
### Trajectory controller ### Trajectory controller
@ -96,12 +96,10 @@ class Main:
self.z2 = np.zeros([5,1]) # [vx;vy;vz;thetadot;phidot] - alpha self.z2 = np.zeros([5,1]) # [vx;vy;vz;thetadot;phidot] - alpha
# Tuning gains # Tuning gains
self.K1 = np.identity(3)#*0.1 self.K1 = np.identity(3)
# self.K1 = np.array([[2,-1,0],[-1,2,-1],[0,-1,2]]) self.K2 = np.identity(5)
self.tune_array = np.array([1,1,1,0.1,0.1]).reshape(5,1) self.tune = 0.1 # Tuning parameter
self.K2 = np.identity(5)#*self.tune_array self.dist = np.array([0,0,0,0.1,0.1]) # Wind disturbance
self.tune = 0.1 #1 # Tuning parameter
self.dist = np.array([0,0,0,0.1,0.1]) # Wind disturbance # np.array([0,0,0,0.01,0.01])
# Gain terms # Gain terms
self.Kp = np.identity(3) + np.dot(self.K2[:3,:3],self.K1) + self.tune*np.identity(3) self.Kp = np.identity(3) + np.dot(self.K2[:3,:3],self.K1) + self.tune*np.identity(3)
self.Kd = self.tot_m*self.K1 + self.tune*self.K2[:3,:3] self.Kd = self.tot_m*self.K1 + self.tune*self.K2[:3,:3]
@ -109,10 +107,8 @@ class Main:
# PD Thrust Controller terms # PD Thrust Controller terms
# gains for thrust PD Controller # gains for thrust PD Controller
#self.Kp = 3.0 self.Kp_thrust = 1.5
#self.Kd = 3 self.Kd_thrust = 1.0
self.Kp_thrust = 1.5 #3.0 #1.5
self.Kd_thrust = 1.0 #3.0 # 1.0
self.R = np.empty([3,3]) # rotation matrix self.R = np.empty([3,3]) # rotation matrix
self.e3 = np.array([[0],[0],[1]]) self.e3 = np.array([[0],[0],[1]])
# Get scaling thrust factor, kf # Get scaling thrust factor, kf
@ -125,7 +121,6 @@ class Main:
# Topic, msg type, and class callback method # Topic, msg type, and class callback method
rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb)
rospy.Subscriber('/reference/path', RefSignal, self.refsig_cb) rospy.Subscriber('/reference/path', RefSignal, self.refsig_cb)
# rospy.Subscriber('/status/twoBody_status', TetheredStatus, self.dronePos_cb)
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb)
rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb)
rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
@ -187,13 +182,20 @@ class Main:
return [drone_m, pl_m] return [drone_m, pl_m]
def get_kf(self): def get_kf(self):
if rospy.has_param('mocap/hover_throttle'): if rospy.has_param('status/hover_throttle'):
hover_throttle = rospy.get_param('mocap/hover_throttle') hover_throttle = rospy.get_param('status/hover_throttle')
else: else:
hover_throttle = (self.tot_m*9.81 + 9.57)/34.84 # linear scaling depending on dependent on mass hover_throttle = (self.tot_m*9.81 + 9.57)/34.84 # linear scaling depending on dependent on mass
kf = hover_throttle/(self.tot_m*9.81) kf = hover_throttle/(self.tot_m*9.81)
return kf return kf
# Check if vehicle has tether
def tether_check(self):
if self.tether == True:
rospy.loginfo_once('TETHER LENGTH: %.2f', self.tetherL)
else:
rospy.loginfo_once('NO TETHER DETECTED')
# --------------------------------------------------------------------------------# # --------------------------------------------------------------------------------#
# CALLBACK FUNCTIONS # CALLBACK FUNCTIONS
# --------------------------------------------------------------------------------# # --------------------------------------------------------------------------------#
@ -243,13 +245,6 @@ class Main:
except ValueError: except ValueError:
pass pass
# Check if vehicle has tether
def tether_check(self):
if self.tether == True:
rospy.loginfo_once('TETHER LENGTH: %.2f', self.tetherL)
else:
rospy.loginfo_once('NO TETHER DETECTED')
def waypoints_srv_cb(self): def waypoints_srv_cb(self):
if '/status/waypoint_tracker' in self.service_list: if '/status/waypoint_tracker' in self.service_list:
rospy.wait_for_service('/status/waypoint_tracker') rospy.wait_for_service('/status/waypoint_tracker')
@ -358,17 +353,9 @@ class Main:
# determine Rotation Matrix # determine Rotation Matrix
self.R_e3 = np.array([[self.R.T[2][0]],[self.R.T[2][1]],[self.R.T[2][2]]]) self.R_e3 = np.array([[self.R.T[2][0]],[self.R.T[2][1]],[self.R.T[2][2]]])
# test which one is better:
# thrust_vector = (9.81*self.tot_m*self.e3 + self.Kp_thrust*self.error + self.Kd_thrust*self.error_vel - self.tot_m*self.path_acc)*self.kf
# thrust = thrust_vector[0]*self.R_e3[0] + thrust_vector[1]*self.R_e3[1] + thrust_vector[2]*self.R_e3[2]
### OR:
thrust_vector = (9.81*self.tot_m + self.Kp_thrust*self.error[2] + self.Kd_thrust*self.error_vel[2] - self.tot_m*self.path_acc[2])*self.kf thrust_vector = (9.81*self.tot_m + self.Kp_thrust*self.error[2] + self.Kd_thrust*self.error_vel[2] - self.tot_m*self.path_acc[2])*self.kf
# thrust = thrust_vector/(math.cos(self.EulerAng[0]*self.EulerAng[1])) #####
thrust = thrust_vector/(math.cos(self.EulerPose[0])*math.cos(self.EulerPose[1])) thrust = thrust_vector/(math.cos(self.EulerPose[0])*math.cos(self.EulerPose[1]))
# if given Fd...?
# thrust = self.kf*Fd/(math.cos(self.EulerPose[0])*math.cos(self.EulerPose[1]))
# Value needs to be between 0 - 1.0 # Value needs to be between 0 - 1.0
self.att_targ.thrust = max(0.0,min(thrust,1.0)) self.att_targ.thrust = max(0.0,min(thrust,1.0))
@ -458,16 +445,11 @@ class Main:
dr_orientation = [self.dr_pos.orientation.x, self.dr_pos.orientation.y, self.dr_pos.orientation.z, self.dr_pos.orientation.w] dr_orientation = [self.dr_pos.orientation.x, self.dr_pos.orientation.y, self.dr_pos.orientation.z, self.dr_pos.orientation.w]
dr_orientation_inv = quaternion_inverse(dr_orientation) dr_orientation_inv = quaternion_inverse(dr_orientation)
# fix_force = self.path_acc
# fix_force = fix_force[:3].reshape(3,1)
# fix_force[0] = self.path_acc[1]
# fix_force[1] = self.path_acc[0]
# Desired body-oriented forces # Desired body-oriented forces
# shouldnt it be tot_m*path_acc?
Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(self.Kd,z1_dot) - np.dot(self.Kp,self.z1) - np.dot(self.Ki,0.5*self.dt*(self.z1 - self.z1_p)) Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(self.Kd,z1_dot) - np.dot(self.Kp,self.z1) - np.dot(self.Ki,0.5*self.dt*(self.z1 - self.z1_p))
# Fd = B + G[:3] + self.tot_m*fix_force - np.dot(self.Kd,z1_dot) - np.dot(self.Kp,self.z1) - np.dot(self.Ki,0.5*self.dt*(self.z1 - self.z1_p))
# Update self.z1_p for "integration" # Update self.z1_p for integration
self.z1_p = self.z1 self.z1_p = self.z1
# Covert Fd into drone frame # Covert Fd into drone frame
@ -479,11 +461,9 @@ class Main:
self.EulerAng[1] = math.atan(Fd_tf[0]/(self.drone_m*9.81)) # Pitch self.EulerAng[1] = math.atan(Fd_tf[0]/(self.drone_m*9.81)) # Pitch
self.EulerAng[0] = math.atan(-Fd_tf[1]*math.cos(self.EulerAng[1])/(self.drone_m*9.81)) # Roll self.EulerAng[0] = math.atan(-Fd_tf[1]*math.cos(self.EulerAng[1])/(self.drone_m*9.81)) # Roll
# rospy.loginfo('Fd before transform: %.2f, %.2f, %.2f' % Fd[0],Fd[1],Fd[2])
q = quaternion_from_euler(self.EulerAng[0],self.EulerAng[1],self.EulerAng[2]) q = quaternion_from_euler(self.EulerAng[0],self.EulerAng[1],self.EulerAng[2])
self.user_fback(Fd,Fd_tf) self.user_feedback(Fd,Fd_tf)
# Populate msg variable # Populate msg variable
# Attitude control # Attitude control
@ -495,7 +475,7 @@ class Main:
self.att_targ.orientation.z = q[2] self.att_targ.orientation.z = q[2]
self.att_targ.orientation.w = q[3] self.att_targ.orientation.w = q[3]
def user_fback(self,F_noTransform, F_Transform): def user_feedback(self,F_noTransform, F_Transform):
print('\n') print('\n')
rospy.loginfo('thrust: %.6f' % self.att_targ.thrust) rospy.loginfo('thrust: %.6f' % self.att_targ.thrust)
rospy.loginfo('roll: %.2f pitch: %.2f',self.EulerAng[0]*180/3.14,self.EulerAng[1]*180/3.14) rospy.loginfo('roll: %.2f pitch: %.2f',self.EulerAng[0]*180/3.14,self.EulerAng[1]*180/3.14)

194
src/mocap_offb_node.cpp
View File

@ -1,194 +0,0 @@
/**
* @file offb_node.cpp
* @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
* Stack and tested in Gazebo SITL
*/
#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/Thrust.h>
#include <mavros_msgs/VFR_HUD.h>
#include <oscillation_ctrl/WaypointTrack.h>
#include <tf2/LinearMath/Quaternion.h>
#include <iostream>
/********* CALLBACK FUNCTIONS **********************/
// Initiate variables
mavros_msgs::State current_state;
geometry_msgs::PoseStamped desPose;
// Callback function which will save the current state of the autopilot.
// Allows to check connection, arming, and Offboard tags*/
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}
// Cb to recieve pose information
// Initiate variables
geometry_msgs::PoseStamped pose;
geometry_msgs::Quaternion q_init;
geometry_msgs::PoseStamped mavPose;
bool pose_read = false;
double current_altitude;
void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
mavPose = *msg;
current_altitude = mavPose.pose.position.z;
while(pose_read == false){
q_init = mavPose.pose.orientation;
if(q_init.x == 0.0 && q_init.w == 0.0){
ROS_INFO("Waiting...");
} else {
mavPose.pose.orientation.x = q_init.x;
mavPose.pose.orientation.y = q_init.y;
mavPose.pose.orientation.z = q_init.z;
mavPose.pose.orientation.w = q_init.w;
pose_read = true;
}
}
}
std_msgs::Bool connection_status;
// bool connection_status
// Determine if we are still receiving info from mocap and land if not
void connection_cb(const std_msgs::Bool::ConstPtr& msg){
connection_status = *msg;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "offb_node");
ros::NodeHandle nh;
/********************** SUBSCRIBERS **********************/
// Get current state
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
// Pose subscriber
ros::Subscriber mavPose_sub = nh.subscribe<geometry_msgs::PoseStamped>
("mavros/local_position/pose",10,mavPose_cb);
ros::Subscriber connection_sub = nh.subscribe<std_msgs::Bool>
("/status/comms",10,connection_cb);
// Waypoint Subscriber
/*
ros::Subscriber waypoint_sub = nh.subscribe<geometry_msgs::PoseStamped>
("/reference/waypoints",10,waypoints_cb);
*/
/********************** PUBLISHERS **********************/
// Initiate publisher to publish commanded local position
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
// Publish desired attitude
ros::Publisher thrust_pub = nh.advertise<mavros_msgs::Thrust>
("mavros/setpoint_attitude/thrust", 10);
// Publish attitude commands
ros::Publisher att_pub = nh.advertise<geometry_msgs::PoseStamped>
("/mavros/setpoint_attitude/attitude",10);
// Service Clients
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
("mavros/cmd/takeoff");
ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
("status/waypoint_tracker");
//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
ros::Rate rate(20.0);
// wait for FCU connection
while(ros::ok() && !current_state.connected){
ros::spinOnce();
rate.sleep();
}
// Retrieve desired waypoints
oscillation_ctrl::WaypointTrack wpoint_srv;
wpoint_srv.request.query = false;
if (waypoint_cl.call(wpoint_srv))
{
ROS_INFO("Waypoints received");
}
// populate desired waypoints
pose.pose.position.x = wpoint_srv.response.xd.x;
pose.pose.position.y = wpoint_srv.response.xd.y;
pose.pose.position.z = wpoint_srv.response.xd.z;
//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
while(ros::ok()){
if(current_state.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(7.0))){
if( set_mode_client.call(offb_set_mode) &&
offb_set_mode.response.mode_sent){
ROS_INFO("OFFBOARD");
}
last_request = ros::Time::now();
} else {
if( !current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(3.0))){
if(arming_client.call(arm_cmd) &&
arm_cmd.response.success){
ROS_INFO("ARMED");
}
last_request = ros::Time::now();
}
}
// Update desired waypoints
waypoint_cl.call(wpoint_srv);
pose.pose.position.x = wpoint_srv.response.xd.x;
pose.pose.position.y = wpoint_srv.response.xd.y;
pose.pose.position.z = wpoint_srv.response.xd.z;
// User info
ROS_INFO("Current Altitude: %.2f",mavPose.pose.position.z);
ROS_INFO("Desired Altitude: %.2f",pose.pose.position.z);
ROS_INFO("---------------------------");
// Check if we are still connected. Otherwise drone should be booted from offboard mode
if(connection_status.data) {
local_pos_pub.publish(pose);
}
else {
ROS_INFO("Connection lost: landing drone...");
}
ros::spinOnce();
rate.sleep();
}
return 0;
}

37
src/mocap_runTest.py
View File

@ -20,13 +20,23 @@ class Main:
self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack) self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
# Set up desired waypoints for test # Set up desired waypoints for test
self.xd = Point() self.xd1 = Point()
self.xd.x = 0.0 self.xd1.x = 0.0
self.xd.y = 2.0 self.xd1.y = -0.5
self.xd.z = 1.5 self.xd1.z = 1.75
self.xd2 = Point()
self.xd2.x = 0.0
self.xd2.y = 2.0
self.xd2.z = 1.75
# Determine if we want to run test with or without controller # Determine if we want to run test with or without controller
################# CHANGE THIS TO CHANGE TYPE Of TEST ###############################
self.change_mode = True # True = Change to oscillation damping mode after wait time self.change_mode = True # True = Change to oscillation damping mode after wait time
self.multiple_setpoins = True # True - will send multiple setpoints
#####################################################################################
if self.change_mode: self.loginfo_string = 'Attitude mode in...' if self.change_mode: self.loginfo_string = 'Attitude mode in...'
else: self.loginfo_string = 'Staying in Position mode.' else: self.loginfo_string = 'Staying in Position mode.'
@ -49,6 +59,7 @@ class Main:
""" Waits desired amount before setting UAV to appropriate mode, and then sets up desired waypoints""" """ Waits desired amount before setting UAV to appropriate mode, and then sets up desired waypoints"""
run_test = False run_test = False
use_ctrl = False use_ctrl = False
waypoint_sent = False
while not run_test: while not run_test:
time_left = self.wait_time - (rospy.get_time() - self.tstart) time_left = self.wait_time - (rospy.get_time() - self.tstart)
if not rospy.get_time() - self.tstart > self.wait_time: if not rospy.get_time() - self.tstart > self.wait_time:
@ -58,15 +69,21 @@ class Main:
self.t_param = rospy.get_time() self.t_param = rospy.get_time()
use_ctrl = True use_ctrl = True
if use_ctrl: if use_ctrl:
time_until_test = 7.0 - rospy.get_time() + self.t_param time_until_test1 = 25.0 - rospy.get_time() + self.t_param
if not time_until_test <= 0.0: time_until_test2 = 30.0 - rospy.get_time() + self.t_param
rospy.loginfo('In %.2f\nSending waypoints: x = %.2f y = %.2f z = %.2f',time_until_test,self.xd.x,self.xd.y,self.xd.z) if time_until_test1 >= 0.0 and not waypoint_sent:
else: rospy.loginfo('In %.2f\nSending waypoints: x = %.2f y = %.2f z = %.2f',time_until_test1,self.xd1.x,self.xd1.y,self.xd1.z)
self.set_waypoint(self.xd) elif not waypoint_sent and time_until_test1 < 0.0:
waypoint_sent = True
self.set_waypoint(self.xd1)
if time_until_test2 >= 0.0 and waypoint_sent:
rospy.loginfo('In %.2f\nSending waypoints: x = %.2f y = %.2f z = %.2f',time_until_test2,self.xd2.x,self.xd2.y,self.xd2.z)
elif waypoint_sent and time_until_test2 < 0.0:
self.set_waypoint(self.xd2)
run_test = True run_test = True
break break
def set_waypoint(self,xd): def set_waypoint(self,xd):
""" Set waypoints for oscillation controller """ """ Set waypoints for oscillation controller """
rospy.wait_for_service('/status/waypoint_tracker') rospy.wait_for_service('/status/waypoint_tracker')

185
src/offb_node.cpp
View File

@ -1,185 +0,0 @@
/**
* @file offb_node.cpp
* @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
* Stack and tested in Gazebo SITL
*/
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/Thrust.h>
#include <mavros_msgs/VFR_HUD.h>
#include <oscillation_ctrl/WaypointTrack.h>
#include <tf2/LinearMath/Quaternion.h>
#include <iostream>
/********* CALLBACK FUNCTIONS **********************/
// Initiate variables
mavros_msgs::State current_state;
geometry_msgs::PoseStamped desPose;
// Callback function which will save the current state of the autopilot.
// Allows to check connection, arming, and Offboard tags*/
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}
// Cb to recieve pose information
// Initiate variables
geometry_msgs::PoseStamped pose;
geometry_msgs::Quaternion q_init;
geometry_msgs::PoseStamped mavPose;
bool pose_read = false;
double current_altitude;
void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
mavPose = *msg;
current_altitude = mavPose.pose.position.z;
while(pose_read == false){
q_init = mavPose.pose.orientation;
if(q_init.x == 0.0 && q_init.w == 0.0){
ROS_INFO("Waiting...");
} else {
pose_read = true;
// pose.pose.orientation.x = q_init.x;
// pose.pose.orientation.y = q_init.y;
// pose.pose.orientation.z = q_init.z;
// pose.pose.orientation.w = q_init.w;
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "offb_node");
ros::NodeHandle nh;
/********************** SUBSCRIBERS **********************/
// Get current state
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
// Pose subscriber
ros::Subscriber mavPose_sub = nh.subscribe<geometry_msgs::PoseStamped>
("mavros/local_position/pose",10,mavPose_cb);
// Waypoint Subscriber
/*
ros::Subscriber waypoint_sub = nh.subscribe<geometry_msgs::PoseStamped>
("/reference/waypoints",10,waypoints_cb);
*/
ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
("status/waypoint_tracker");
/********************** PUBLISHERS **********************/
// Initiate publisher to publish commanded local position
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
// Publish desired attitude
ros::Publisher thrust_pub = nh.advertise<mavros_msgs::Thrust>
("mavros/setpoint_attitude/thrust", 10);
// Publish attitude commands
ros::Publisher att_pub = nh.advertise<geometry_msgs::PoseStamped>
("/mavros/setpoint_attitude/attitude",10);
// Service Clients
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
("mavros/cmd/takeoff");
//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
ros::Rate rate(20.0);
// wait for FCU connection
while(ros::ok() && !current_state.connected){
ros::spinOnce();
rate.sleep();
}
// Retrieve desired waypoints
oscillation_ctrl::WaypointTrack wpoint_srv;
wpoint_srv.request.query = false;
if (waypoint_cl.call(wpoint_srv))
{
ROS_INFO("Waypoints received");
}
// populate desired waypoints
pose.pose.position.x = wpoint_srv.response.xd.x;
pose.pose.position.y = wpoint_srv.response.xd.y;
pose.pose.position.z = wpoint_srv.response.xd.z;
/*/ Populate pose msg
pose.pose.orientation.x = q_init.x;
pose.pose.orientation.y = q_init.y;
pose.pose.orientation.z = q_init.z;
pose.pose.orientation.w = q_init.w;
*/
//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
while(ros::ok()){
if(current_state.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( set_mode_client.call(offb_set_mode) &&
offb_set_mode.response.mode_sent){
ROS_INFO("Offboard enabled");
}
last_request = ros::Time::now();
} else {
if( !current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(3.0))){
if(arming_client.call(arm_cmd) &&
arm_cmd.response.success){
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
}
// check if waypoints have changed desired waypoints
waypoint_cl.call(wpoint_srv);
pose.pose.position.x = wpoint_srv.response.xd.x;
pose.pose.position.y = wpoint_srv.response.xd.y;
pose.pose.position.z = wpoint_srv.response.xd.z;
// User info
ROS_INFO("Current Altitude: %.2f",mavPose.pose.position.z);
ROS_INFO("Desired Altitude: %.2f",pose.pose.position.z);
ROS_INFO("---------------------------");
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
return 0;
}

47
src/path_follow.cpp
View File

@ -44,18 +44,18 @@ double current_altitude;
void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
mavPose = *msg; mavPose = *msg;
current_altitude = mavPose.pose.position.z; current_altitude = mavPose.pose.position.z;
while(gps_read == false){ // while(gps_read == false){
q_init = mavPose.pose.orientation; // q_init = mavPose.pose.orientation;
if(q_init.x == 0.0 && q_init.w == 0.0){ // if(q_init.x == 0.0 && q_init.w == 0.0){
ROS_INFO("Waiting..."); // ROS_INFO("Waiting...");
} else { // } else {
buff_pose.pose.orientation.x = q_init.x; // buff_pose.pose.orientation.x = q_init.x;
buff_pose.pose.orientation.y = q_init.y; // buff_pose.pose.orientation.y = q_init.y;
buff_pose.pose.orientation.z = q_init.z; // buff_pose.pose.orientation.z = q_init.z;
buff_pose.pose.orientation.w = q_init.w; // buff_pose.pose.orientation.w = q_init.w;
gps_read = true; // gps_read = true;
} // }
} // }
} }
int main(int argc, char **argv) int main(int argc, char **argv)
@ -154,14 +154,11 @@ int main(int argc, char **argv)
double alt_des = wpoint_srv.response.xd.z; // Desired height double alt_des = wpoint_srv.response.xd.z; // Desired height
while(ros::ok()){ while(ros::ok()){
if(gps_read == true){
ROS_INFO("Entered while loop");
if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){ if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){
if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){
} else { } else {
ROS_INFO("Could not enter offboard mode"); ROS_INFO("Could not enter offboard mode");
} }
//last_request = ros::Time::now();
} else { } else {
if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){ if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){
if( arming_client.call(arm_cmd) && arm_cmd.response.success){ if( arming_client.call(arm_cmd) && arm_cmd.response.success){
@ -176,7 +173,6 @@ int main(int argc, char **argv)
} }
// Check if we want to use oscillation controller // Check if we want to use oscillation controller
//if (ros::param::get("/use_ctrl", oscillation_damp) == true){
if (ros::param::has("/status/use_ctrl")){ if (ros::param::has("/status/use_ctrl")){
ros::param::get("/status/use_ctrl", oscillation_damp); ros::param::get("/status/use_ctrl", oscillation_damp);
if(oscillation_damp == true){ if(oscillation_damp == true){
@ -185,28 +181,21 @@ int main(int argc, char **argv)
// Publish attitude commands // Publish attitude commands
att_targ_pub.publish(att_targ); att_targ_pub.publish(att_targ);
} else { } else {
// Check if waypoints have changed
// For attitude controller, ref_signalGen deals with changes
// in desired waypoints, so we only check if not using controller
if (waypoint_cl.call(wpoint_srv))
{
// populate desired waypoints
buff_pose.pose.position.x = wpoint_srv.response.xd.x;
buff_pose.pose.position.y = wpoint_srv.response.xd.y;
buff_pose.pose.position.z = wpoint_srv.response.xd.z;
}
ROS_INFO("POSITION CTRL"); ROS_INFO("POSITION CTRL");
//ROS_INFO("POSITION CTRL");
// Publish position setpoints // Publish position setpoints
local_pos_pub.publish(buff_pose); local_pos_pub.publish(buff_pose);
} }
} ROS_INFO("Des Altitude: %.2f", wpoint_srv.response.xd.z);
ROS_INFO("Des Altitude: %.2f", alt_des);
ROS_INFO("Cur Altitude: %.2f", current_altitude); ROS_INFO("Cur Altitude: %.2f", current_altitude);
ROS_INFO("---------------------------"); ROS_INFO("---------------------------");
} else {
ROS_WARN("CONTROL PARAM NOT FOUND. PLEASE SET '/status/use_ctrl'");
}
ros::spinOnce(); ros::spinOnce();
rate_pub.sleep(); rate_pub.sleep();
} }
}
return 0; return 0;
} }

38
src/ref_signalGen.py
View File

@ -1,4 +1,4 @@
#!/usr/bin/env python2.7 #!/usr/bin/env python
### Cesar Rodriguez July 2021 ### Cesar Rodriguez July 2021
### Based off of Klausen 2017 - Smooth trajectory generation based on desired waypoints ### Based off of Klausen 2017 - Smooth trajectory generation based on desired waypoints
@ -10,11 +10,10 @@ import math
import rosservice import rosservice
from scipy.integrate import odeint from scipy.integrate import odeint
from oscillation_ctrl.msg import RefSignal, LoadAngles, TetheredStatus from oscillation_ctrl.msg import RefSignal, LoadAngles
from oscillation_ctrl.srv import WaypointTrack from oscillation_ctrl.srv import WaypointTrack
from geometry_msgs.msg import Pose, PoseStamped, Point, TwistStamped from geometry_msgs.msg import Pose, PoseStamped, Point, TwistStamped
from sensor_msgs.msg import Imu from sensor_msgs.msg import Imu
from mavros_msgs.msg import State
class DesiredPoint(): class DesiredPoint():
def __init__(self,x,y,z): def __init__(self,x,y,z):
@ -38,7 +37,6 @@ class Main:
self.tmax = self.dt # used to get desired pos, vel, and acc for next time step (tmax) self.tmax = self.dt # used to get desired pos, vel, and acc for next time step (tmax)
self.n = self.tmax/self.dt + 1 self.n = self.tmax/self.dt + 1
self.t = np.linspace(0, self.tmax, self.n) # Time array self.t = np.linspace(0, self.tmax, self.n) # Time array
# self.t = np.linspace(0, 1.0, 10) # Time array
# Message generation/ collection # Message generation/ collection
self.vel_data = TwistStamped() # This is needed to get drone vel from gps self.vel_data = TwistStamped() # This is needed to get drone vel from gps
@ -58,7 +56,6 @@ class Main:
# Topic, msg type, and class callback method # Topic, msg type, and class callback method
rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb)
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb)
# rospy.Subscriber('/status/twoBody_status', TetheredStatus, self.dronePos_cb)
rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb)
rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
@ -67,8 +64,7 @@ class Main:
# --------------------------------------------------------------------------------# # --------------------------------------------------------------------------------#
# Publish desired path to compute attitudes # Publish desired path to compute attitudes
self.pub_path = rospy.Publisher('/reference/path',RefSignal,queue_size = 10) self.pub_path = rospy.Publisher('/reference/path',RefSignal,queue_size = 10)
# Needed for geometric controller to compute thrust self.waypointTracker_pub = rospy.Publisher('/reference/waypoints',Point,queue_size = 10) # not needed. Used for performance analysis
#self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10)
# timer(s), used to control method loop freq(s) as defined by the rate(s) # timer(s), used to control method loop freq(s) as defined by the rate(s)
self.pub_tim = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) self.pub_tim = rospy.Timer(rospy.Duration(1.0/rate), self.publisher)
@ -82,7 +78,7 @@ class Main:
self.EPS_I = np.zeros(9) # Epsilon shapefilter self.EPS_I = np.zeros(9) # Epsilon shapefilter
# Constants for smooth path generation # Constants for smooth path generation
self.w_tune = 1 # also works well :) 3.13 works well? ######################################################################### self.w_tune = 1 # Increase this to increase aggresiveness of trajectory i.e. higher accelerations
self.epsilon = 0.7 # Damping ratio self.epsilon = 0.7 # Damping ratio
# need exception if we do not have tether: # need exception if we do not have tether:
@ -330,9 +326,6 @@ class Main:
self.sigmoid() # Determine sigmoid gain self.sigmoid() # Determine sigmoid gain
EPS_D = self.fback() # Feedback Epsilon EPS_D = self.fback() # Feedback Epsilon
# for i in range(9):
# Populate EPS_F buffer with desired change based on feedback
# self.EPS_F[i] = self.EPS_I[i] + self.s_gain*EPS_D[i] #+ EPS_D[i]
self.EPS_F = self.EPS_I + self.s_gain*EPS_D self.EPS_F = self.EPS_I + self.s_gain*EPS_D
# Populate msg with epsilon_final # Populate msg with epsilon_final
@ -340,22 +333,14 @@ class Main:
#self.ref_sig.type_mask = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin #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.x = self.EPS_F[0]
self.ref_sig.position.y = self.EPS_F[1] self.ref_sig.position.y = self.EPS_F[1]
self.ref_sig.position.z = self.EPS_F[2]
self.ref_sig.velocity.x = self.EPS_F[3] self.ref_sig.velocity.x = self.EPS_F[3]
self.ref_sig.velocity.y = self.EPS_F[4] 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.x = self.EPS_F[6]
self.ref_sig.acceleration.y = self.EPS_F[7] self.ref_sig.acceleration.y = self.EPS_F[7]
# Do not need to evaluate z
# self.ref_sig.position.z = self.xd.z
# self.ref_sig.velocity.z = 0.0
# self.ref_sig.acceleration.z = 0.0
self.ref_sig.position.z = self.EPS_F[2]
self.ref_sig.velocity.z = self.EPS_F[5]
self.ref_sig.acceleration.z = self.EPS_F[8] self.ref_sig.acceleration.z = self.EPS_F[8]
# self.x0 = [self.dr_pos.position.x, self.x[1,1], self.x[1,2], self.x[1,3]]
# self.y0 = [self.dr_pos.position.y, self.y[1,1], self.y[1,2], self.y[1,3]]
# self.z0 = [self.dr_pos.position.z, self.z[1,1], self.z[1,2], self.z[1,3]]
self.x0 = [self.x[1,0], self.x[1,1], self.x[1,2], self.x[1,3]] self.x0 = [self.x[1,0], self.x[1,1], self.x[1,2], self.x[1,3]]
self.y0 = [self.y[1,0], self.y[1,1], self.y[1,2], self.y[1,3]] self.y0 = [self.y[1,0], self.y[1,1], self.y[1,2], self.y[1,3]]
self.z0 = [self.z[1,0], self.z[1,1], self.z[1,2], self.z[1,3]] self.z0 = [self.z[1,0], self.z[1,1], self.z[1,2], self.z[1,3]]
@ -379,7 +364,7 @@ class Main:
return EPS_D return EPS_D
def user_fback(self): def user_feeback(self):
# Feedback to user # Feedback to user
rospy.loginfo(' Var | x | y | z ') rospy.loginfo(' Var | x | y | z ')
@ -387,9 +372,7 @@ class Main:
rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5]) rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5])
rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8]) rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8])
rospy.loginfo('_______________________') rospy.loginfo('_______________________')
# rospy.loginfo_once('Tether length: %.2f',self.tetherL)
# rospy.loginfo('Theta: %.2f',self.load_angles.theta)
# rospy.loginfo('Phi: %.2f',self.load_angles.phi)
def publisher(self,pub_tim): def publisher(self,pub_tim):
@ -399,8 +382,11 @@ class Main:
# Publish reference signal # Publish reference signal
self.pub_path.publish(self.ref_sig) self.pub_path.publish(self.ref_sig)
# Publish what the setpoints are
self.waypointTracker_pub.publish(self.xd)
# Give user feedback on published message: # Give user feedback on published message:
self.user_fback() self.user_feeback()
if __name__=="__main__": if __name__=="__main__":

59
src/set_ploadmass.py Executable file
View File

@ -0,0 +1,59 @@
#!/usr/bin/env python2.7
### Cesar Rodriguez Sept 2022
### changes pload mass depending on /status/pload_mass parameter
import rospy
import rosservice
from gazebo_msgs.srv import GetLinkProperties, SetLinkProperties
class Main:
def __init__(self):
# Variables needed for testing start
# link (payload) name
self.link_name = 'spiri_with_tether::mass::payload'
# service names we will use
self.service_request = '/gazebo/get_link_properties'
self.service_call = '/gazebo/set_link_properties'
# wait for service
rospy.wait_for_service(self.service_request)
# set up service request
get_link_properties = rospy.ServiceProxy(self.service_request,GetLinkProperties)
# wait for service to set link properties
rospy.wait_for_service(self.service_call)
# set up service request
set_link_properties = rospy.ServiceProxy(self.service_call,SetLinkProperties)
# get payload properties
self.pload_properties = get_link_properties(self.link_name)
# state parameter name for where payload mass is stored
self.param_pload_mass = 'status/pload_mass'
# check if parameter exists
if rospy.has_param(self.param_pload_mass):
self.desired_mass = rospy.get_param(self.param_pload_mass) # get desired mass
try:
# set desired mass and necessary inertial properties
set_link_properties(link_name=self.link_name,mass=self.desired_mass,
ixx=self.pload_properties.ixx,iyy=self.pload_properties.iyy,izz=self.pload_properties.izz,
gravity_mode=self.pload_properties.gravity_mode)
rospy.loginfo('Set payload mass to: %.3f', self.desired_mass)
except rospy.ServiceException as e:
rospy.loginfo("Set Link State call failed: {0}".format(e))
else: rospy.logwarn('COULD NOT FIND PLOAD MASS PARAM')
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

173
src/thrust_controller.py
View File

@ -1,173 +0,0 @@
#!/usr/bin/env python2.7
### Cesar Rodriguez Sept 21
### Used to control thrust of drone
import rospy, tf
import numpy as np
from geometry_msgs.msg import PoseStamped, TwistStamped
from mavros_msgs.msg import AttitudeTarget
class PID:
def __init__(self):
# rate(s)
rate = 35.0
self.dt = 1/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()
# tuning gains
#self.Kp = 0.335
#self.Kd = 0.1
self.Kp = 2.7
self.Kd = 0.5
# drone var
self.drone_m = 1.437
self.max_a = 14.2
self.max_t = self.drone_m*self.max_a
# message variables
self.pose = PoseStamped()
self.pose_buff = PoseStamped()
self.pose_buff.pose.position.z = 2.5
self.attitude = AttitudeTarget()
self.attitude.type_mask = 1|2|4 # ignore body rate command
self.attitude.orientation.x = 0.0
self.attitude.orientation.y = 0.0
self.attitude.orientation.z = 0.0
self.attitude.orientation.w = 1.0
self.des_alt = 2.5
self.dr_vel = TwistStamped()
#self.cur_att = PoseStamped()
self.R = np.empty([3,3])
# clients
# subscribers
rospy.Subscriber('/mavros/local_position/pose',PoseStamped, self.pose_cb)
rospy.Subscriber('/mavros/local_position/velocity_local', TwistStamped, self.droneVel_cb)
# publishers
self.pub_attitude = rospy.Publisher('/command/thrust', AttitudeTarget, queue_size=10)
# publishing rate
self.pub_time = rospy.Timer(rospy.Duration(1.0/rate), self.publisher)
def pose_cb(self,msg):
self.cur_alt = msg.pose.position.z
self.pose = msg
# Callback for drone velocity
def droneVel_cb(self,msg):
try:
self.dr_vel = msg
except ValueError or TypeError:
pass
def quaternion_rotation_matrix(self):
"""
Covert a quaternion into a full three-dimensional rotation matrix.
Input
:param Q: A 4 element array representing the quaternion (q0,q1,q2,q3)
Output
:return: A 3x3 element matrix representing the full 3D rotation matrix.
This rotation matrix converts a point in the local reference
frame to a point in the global reference frame.
"""
# Extract the values from Q
q0 = self.pose.pose.orientation.w
q1 = self.pose.pose.orientation.x
q2 = self.pose.pose.orientation.y
q3 = self.pose.pose.orientation.z
# First row of the rotation matrix
r00 = 2 * (q0 * q0 + q1 * q1) - 1
r01 = 2 * (q1 * q2 - q0 * q3)
r02 = 2 * (q1 * q3 + q0 * q2)
# Second row of the rotation matrix
r10 = 2 * (q1 * q2 + q0 * q3)
r11 = 2 * (q0 * q0 + q2 * q2) - 1
r12 = 2 * (q2 * q3 - q0 * q1)
# Third row of the rotation matrix
r20 = 2 * (q1 * q3 - q0 * q2)
r21 = 2 * (q2 * q3 + q0 * q1)
r22 = 2 * (q0 * q0 + q3 * q3) - 1
# 3x3 rotation matrix
rot_matrix = np.array([[r00, r01, r02],
[r10, r11, r12],
[r20, r21, r22]])
return rot_matrix
def operation(self):
self.error = self.cur_alt - self.des_alt # error in z dir.
self.R = self.quaternion_rotation_matrix() # determine Rotation Matrix
# thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3)
# Taeyoung Lee, Melvin Leok, and N. Harris McClamroch
thrust = np.dot(9.81*self.drone_m -self.Kp*self.error -self.Kd*self.dr_vel.twist.linear.z,self.R.T[2])/self.max_t
self.attitude.thrust = thrust[2] # save thurst in z-dir
# Value needs to be between 0 - 1.0
if self.attitude.thrust >= 1.0:
self.attitude.thrust = 1.0
elif self.attitude.thrust <= 0.0:
self.attitude.thrust = 0.0
def publisher(self,pub_time):
self.operation() # determine thrust
self.attitude.header.stamp = rospy.Time.now()
self.pub_attitude.publish(self.attitude)
def user_feedback(self):
rospy.loginfo('Des: %.2f',self.des_alt)
rospy.loginfo('Error: %.2f',self.error)
rospy.loginfo('Throttle: %f\n',self.attitude.thrust)
if __name__=="__main__":
# Initiate ROS node
rospy.init_node('thrust_PID')
try:
PID() # create class object
rospy.spin() # loop until shutdown signal
except rospy.ROSInterruptException:
pass