Lastest changes to algorithm + adding airframes to work with Ubuntu 20.04
This commit is contained in:
parent
5b224e599d
commit
937fd75fee
|
|
@ -2,10 +2,13 @@ config/mocap_*
|
|||
launch/cortex_bridge.launch
|
||||
launch/debug.launch
|
||||
launch/klausen_dampen.launch
|
||||
launch/mocap_*
|
||||
src/development_*
|
||||
src/killswitch_client.py
|
||||
src/land_client.py
|
||||
src/MoCap_*.py
|
||||
src/Mocap_*.py
|
||||
src/mocap_*
|
||||
src/segmented_tether.py
|
||||
src/segmented_tether_fast.py
|
||||
msg/Marker.msg
|
||||
|
|
|
|||
|
|
@ -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_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
|
||||
# add_library(${PROJECT_NAME}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -16,11 +16,6 @@ param set-default MC_PITCHRATE_D 0.0060
|
|||
param set-default MC_PITCHRATE_I 0.35
|
||||
|
||||
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_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 COM_RCL_EXCEPT 4
|
||||
param set-default NAV_RCL_ACT 1
|
||||
param set-default COM_RCL_EXCEPT 4 # Ignores no RC failsafe (not needed for simulations)
|
||||
|
||||
set MIXER quad_x
|
||||
set PWM_OUT 1234
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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???
|
||||
|
|
@ -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]
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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]
|
||||
|
|
@ -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>
|
||||
|
|
@ -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>
|
||||
|
|
@ -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="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/Ctrl_param.yaml" />
|
||||
<rosparam file="$(find oscillation_ctrl)/config/mocapLab_params.yaml" />
|
||||
<param name="test_type" value="$(arg test)"/>
|
||||
</group>
|
||||
|
||||
|
|
|
|||
|
|
@ -3,12 +3,10 @@
|
|||
Launch file to use klausen oscillaton damping ctrl in Gazebo
|
||||
/-->
|
||||
<launch>
|
||||
<arg name="model" default="headless_spiri_mocap"/>
|
||||
<arg name="ctrl" default="yes"/>
|
||||
<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 ns="status">
|
||||
<rosparam file="$(find oscillation_ctrl)/config/mocapGazebo_params.yaml" />
|
||||
</group>
|
||||
|
||||
<arg name="model" default="headless_spiri"/>
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="MoCap_Localization_fake.py"
|
||||
|
|
@ -21,22 +19,6 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
type="wpoint_tracker.py"
|
||||
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 -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
|
|
@ -64,8 +46,15 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
name="mocap_Test"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
</group>
|
||||
<!-- SETS PLOAD MASS -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="set_ploadmass.py"
|
||||
name="set_ploadmass"
|
||||
output="screen"
|
||||
/>
|
||||
<!-- PX4 LAUNCH -->
|
||||
<include file="$(find px4)/launch/$(arg model).launch"/>
|
||||
<include file="$(find oscillation_ctrl)/launch/$(arg model)_mocap.launch">
|
||||
</include>
|
||||
</launch>
|
||||
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
@ -5,11 +5,9 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
<launch>
|
||||
<arg name="model" default="headless_spiri_with_tether"/>
|
||||
<arg name="test" default="none"/>
|
||||
<group ns="sim">
|
||||
<rosparam file="$(find oscillation_ctrl)/config/gazebo_config.yaml" />
|
||||
</group>
|
||||
|
||||
<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)"/>
|
||||
</group>
|
||||
|
||||
|
|
@ -37,7 +35,6 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
pkg="oscillation_ctrl"
|
||||
type="klausen_control.py"
|
||||
name="klausenCtrl_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
<!-- PUBLISHES DESIRED COMMANDS -->
|
||||
<node
|
||||
|
|
|
|||
|
|
@ -9,7 +9,6 @@ import time
|
|||
import math
|
||||
from tf.transformations import *
|
||||
from oscillation_ctrl.msg import TetheredStatus, LoadAngles
|
||||
from geometry_msgs.msg import Pose
|
||||
from gazebo_msgs.srv import GetLinkState
|
||||
from std_msgs.msg import Bool
|
||||
|
||||
|
|
@ -53,10 +52,6 @@ class Main:
|
|||
self.thetabuf = 0.0 #
|
||||
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)
|
||||
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.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):
|
||||
"""
|
||||
Takes in pose msg object and outputs array of euler angs:
|
||||
|
|
@ -171,7 +147,6 @@ class Main:
|
|||
|
||||
# Determine thetadot
|
||||
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
|
||||
|
||||
# Determine phi (roll)
|
||||
|
|
@ -184,7 +159,6 @@ class Main:
|
|||
|
||||
# Determine phidot
|
||||
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
|
||||
|
||||
else: # Otherwise, vars = 0
|
||||
|
|
@ -204,17 +178,18 @@ class Main:
|
|||
rospy.loginfo("Get Link State call failed: {0}".format(e))
|
||||
|
||||
def user_feedback(self,gui_timer):
|
||||
# Print and save results
|
||||
#print "\n"
|
||||
print ("\n")
|
||||
rospy.loginfo("")
|
||||
print"Roll: "+str(round(self.drone_Eul[0]*180/3.14,2))
|
||||
print"Pitch: " +str(round(self.drone_Eul[1]*180/3.14,2))
|
||||
print"Yaw: " +str(round(self.drone_Eul[2]*180/3.14,2))
|
||||
print "drone pos.x: " + str(round(self.drone_Px,2))
|
||||
print "drone pos.y: " + str(round(self.drone_Py,2))
|
||||
print "drone pos.z: " + str(round(self.drone_Pz,2))
|
||||
print "phi: " + str(round(self.loadAngles.phi*180/3.14,3))
|
||||
print "theta: " + str(round(self.loadAngles.theta*180/3.14,3))
|
||||
print("Drone pos.x: " + str(round(self.drone_Px,2)))
|
||||
print("Drone pos.y: " + str(round(self.drone_Py,2)))
|
||||
print("Drone pos.z: " + str(round(self.drone_Pz,2)))
|
||||
print("Roll: "+str(round(self.drone_Eul[0]*180/3.14,2)))
|
||||
print("Pitch: " +str(round(self.drone_Eul[1]*180/3.14,2)))
|
||||
print("Yaw: " +str(round(self.drone_Eul[2]*180/3.14,2)))
|
||||
if self.pload:
|
||||
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):
|
||||
now = rospy.get_time()
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
@ -1,4 +1,4 @@
|
|||
#!/usr/bin/env python2.7
|
||||
#!/usr/bin/env python
|
||||
|
||||
### Cesar Rodriguez Aug 2021
|
||||
### Trajectory controller
|
||||
|
|
@ -96,12 +96,10 @@ class Main:
|
|||
self.z2 = np.zeros([5,1]) # [vx;vy;vz;thetadot;phidot] - alpha
|
||||
|
||||
# Tuning gains
|
||||
self.K1 = np.identity(3)#*0.1
|
||||
# self.K1 = np.array([[2,-1,0],[-1,2,-1],[0,-1,2]])
|
||||
self.tune_array = np.array([1,1,1,0.1,0.1]).reshape(5,1)
|
||||
self.K2 = np.identity(5)#*self.tune_array
|
||||
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])
|
||||
self.K1 = np.identity(3)
|
||||
self.K2 = np.identity(5)
|
||||
self.tune = 0.1 # Tuning parameter
|
||||
self.dist = np.array([0,0,0,0.1,0.1]) # Wind disturbance
|
||||
# Gain terms
|
||||
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]
|
||||
|
|
@ -109,10 +107,8 @@ class Main:
|
|||
|
||||
# PD Thrust Controller terms
|
||||
# gains for thrust PD Controller
|
||||
#self.Kp = 3.0
|
||||
#self.Kd = 3
|
||||
self.Kp_thrust = 1.5 #3.0 #1.5
|
||||
self.Kd_thrust = 1.0 #3.0 # 1.0
|
||||
self.Kp_thrust = 1.5
|
||||
self.Kd_thrust = 1.0
|
||||
self.R = np.empty([3,3]) # rotation matrix
|
||||
self.e3 = np.array([[0],[0],[1]])
|
||||
# Get scaling thrust factor, kf
|
||||
|
|
@ -125,7 +121,6 @@ class Main:
|
|||
# Topic, msg type, and class callback method
|
||||
rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_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/velocity_body', TwistStamped, self.droneVel_cb)
|
||||
rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
|
||||
|
|
@ -187,13 +182,20 @@ class Main:
|
|||
return [drone_m, pl_m]
|
||||
|
||||
def get_kf(self):
|
||||
if rospy.has_param('mocap/hover_throttle'):
|
||||
hover_throttle = rospy.get_param('mocap/hover_throttle')
|
||||
if rospy.has_param('status/hover_throttle'):
|
||||
hover_throttle = rospy.get_param('status/hover_throttle')
|
||||
else:
|
||||
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)
|
||||
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
|
||||
# --------------------------------------------------------------------------------#
|
||||
|
|
@ -243,13 +245,6 @@ class Main:
|
|||
except ValueError:
|
||||
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):
|
||||
if '/status/waypoint_tracker' in self.service_list:
|
||||
rospy.wait_for_service('/status/waypoint_tracker')
|
||||
|
|
@ -358,17 +353,9 @@ class Main:
|
|||
# determine Rotation Matrix
|
||||
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 = thrust_vector/(math.cos(self.EulerAng[0]*self.EulerAng[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
|
||||
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_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
|
||||
# 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*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
|
||||
|
||||
# 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[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])
|
||||
|
||||
self.user_fback(Fd,Fd_tf)
|
||||
self.user_feedback(Fd,Fd_tf)
|
||||
|
||||
# Populate msg variable
|
||||
# Attitude control
|
||||
|
|
@ -495,7 +475,7 @@ class Main:
|
|||
self.att_targ.orientation.z = q[2]
|
||||
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')
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -20,13 +20,23 @@ class Main:
|
|||
self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
|
||||
|
||||
# Set up desired waypoints for test
|
||||
self.xd = Point()
|
||||
self.xd.x = 0.0
|
||||
self.xd.y = 2.0
|
||||
self.xd.z = 1.5
|
||||
self.xd1 = Point()
|
||||
self.xd1.x = 0.0
|
||||
self.xd1.y = -0.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
|
||||
################# CHANGE THIS TO CHANGE TYPE Of TEST ###############################
|
||||
|
||||
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...'
|
||||
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"""
|
||||
run_test = False
|
||||
use_ctrl = False
|
||||
waypoint_sent = False
|
||||
while not run_test:
|
||||
time_left = self.wait_time - (rospy.get_time() - self.tstart)
|
||||
if not rospy.get_time() - self.tstart > self.wait_time:
|
||||
|
|
@ -58,15 +69,21 @@ class Main:
|
|||
self.t_param = rospy.get_time()
|
||||
use_ctrl = True
|
||||
if use_ctrl:
|
||||
time_until_test = 7.0 - rospy.get_time() + self.t_param
|
||||
if not time_until_test <= 0.0:
|
||||
rospy.loginfo('In %.2f\nSending waypoints: x = %.2f y = %.2f z = %.2f',time_until_test,self.xd.x,self.xd.y,self.xd.z)
|
||||
else:
|
||||
self.set_waypoint(self.xd)
|
||||
time_until_test1 = 25.0 - rospy.get_time() + self.t_param
|
||||
time_until_test2 = 30.0 - rospy.get_time() + self.t_param
|
||||
if time_until_test1 >= 0.0 and not waypoint_sent:
|
||||
rospy.loginfo('In %.2f\nSending waypoints: x = %.2f y = %.2f z = %.2f',time_until_test1,self.xd1.x,self.xd1.y,self.xd1.z)
|
||||
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
|
||||
break
|
||||
|
||||
|
||||
def set_waypoint(self,xd):
|
||||
""" Set waypoints for oscillation controller """
|
||||
rospy.wait_for_service('/status/waypoint_tracker')
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -44,18 +44,18 @@ double current_altitude;
|
|||
void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
|
||||
mavPose = *msg;
|
||||
current_altitude = mavPose.pose.position.z;
|
||||
while(gps_read == false){
|
||||
q_init = mavPose.pose.orientation;
|
||||
if(q_init.x == 0.0 && q_init.w == 0.0){
|
||||
ROS_INFO("Waiting...");
|
||||
} else {
|
||||
buff_pose.pose.orientation.x = q_init.x;
|
||||
buff_pose.pose.orientation.y = q_init.y;
|
||||
buff_pose.pose.orientation.z = q_init.z;
|
||||
buff_pose.pose.orientation.w = q_init.w;
|
||||
gps_read = true;
|
||||
}
|
||||
}
|
||||
// while(gps_read == false){
|
||||
// q_init = mavPose.pose.orientation;
|
||||
// if(q_init.x == 0.0 && q_init.w == 0.0){
|
||||
// ROS_INFO("Waiting...");
|
||||
// } else {
|
||||
// buff_pose.pose.orientation.x = q_init.x;
|
||||
// buff_pose.pose.orientation.y = q_init.y;
|
||||
// buff_pose.pose.orientation.z = q_init.z;
|
||||
// buff_pose.pose.orientation.w = q_init.w;
|
||||
// gps_read = true;
|
||||
// }
|
||||
// }
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
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( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){
|
||||
} else {
|
||||
ROS_INFO("Could not enter offboard mode");
|
||||
}
|
||||
//last_request = ros::Time::now();
|
||||
} else {
|
||||
if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){
|
||||
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
|
||||
//if (ros::param::get("/use_ctrl", oscillation_damp) == true){
|
||||
if (ros::param::has("/status/use_ctrl")){
|
||||
ros::param::get("/status/use_ctrl", oscillation_damp);
|
||||
if(oscillation_damp == true){
|
||||
|
|
@ -185,28 +181,21 @@ int main(int argc, char **argv)
|
|||
// Publish attitude commands
|
||||
att_targ_pub.publish(att_targ);
|
||||
} 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");
|
||||
// Publish position setpoints
|
||||
local_pos_pub.publish(buff_pose);
|
||||
}
|
||||
}
|
||||
ROS_INFO("Des Altitude: %.2f", alt_des);
|
||||
ROS_INFO("Des Altitude: %.2f", wpoint_srv.response.xd.z);
|
||||
ROS_INFO("Cur Altitude: %.2f", current_altitude);
|
||||
ROS_INFO("---------------------------");
|
||||
} else {
|
||||
ROS_WARN("CONTROL PARAM NOT FOUND. PLEASE SET '/status/use_ctrl'");
|
||||
}
|
||||
|
||||
ros::spinOnce();
|
||||
rate_pub.sleep();
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,4 +1,4 @@
|
|||
#!/usr/bin/env python2.7
|
||||
#!/usr/bin/env python
|
||||
|
||||
### Cesar Rodriguez July 2021
|
||||
### Based off of Klausen 2017 - Smooth trajectory generation based on desired waypoints
|
||||
|
|
@ -10,11 +10,10 @@ import math
|
|||
import rosservice
|
||||
|
||||
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 geometry_msgs.msg import Pose, PoseStamped, Point, TwistStamped
|
||||
from sensor_msgs.msg import Imu
|
||||
from mavros_msgs.msg import State
|
||||
|
||||
class DesiredPoint():
|
||||
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.n = self.tmax/self.dt + 1
|
||||
self.t = np.linspace(0, self.tmax, self.n) # Time array
|
||||
# self.t = np.linspace(0, 1.0, 10) # Time array
|
||||
|
||||
# Message generation/ collection
|
||||
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
|
||||
rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_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/imu/data', Imu, self.droneAcc_cb)
|
||||
|
||||
|
|
@ -67,8 +64,7 @@ class Main:
|
|||
# --------------------------------------------------------------------------------#
|
||||
# Publish desired path to compute attitudes
|
||||
self.pub_path = rospy.Publisher('/reference/path',RefSignal,queue_size = 10)
|
||||
# Needed for geometric controller to compute thrust
|
||||
#self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10)
|
||||
self.waypointTracker_pub = rospy.Publisher('/reference/waypoints',Point,queue_size = 10) # not needed. Used for performance analysis
|
||||
|
||||
# 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)
|
||||
|
|
@ -82,7 +78,7 @@ class Main:
|
|||
self.EPS_I = np.zeros(9) # Epsilon shapefilter
|
||||
|
||||
# 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
|
||||
|
||||
# need exception if we do not have tether:
|
||||
|
|
@ -330,9 +326,6 @@ class Main:
|
|||
self.sigmoid() # Determine sigmoid gain
|
||||
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
|
||||
|
||||
# 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.position.x = self.EPS_F[0]
|
||||
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.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]
|
||||
|
||||
# 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.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.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]]
|
||||
|
|
@ -379,7 +364,7 @@ class Main:
|
|||
|
||||
return EPS_D
|
||||
|
||||
def user_fback(self):
|
||||
def user_feeback(self):
|
||||
|
||||
# Feedback to user
|
||||
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('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8])
|
||||
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):
|
||||
|
||||
|
|
@ -399,8 +382,11 @@ class Main:
|
|||
# Publish reference signal
|
||||
self.pub_path.publish(self.ref_sig)
|
||||
|
||||
# Publish what the setpoints are
|
||||
self.waypointTracker_pub.publish(self.xd)
|
||||
|
||||
# Give user feedback on published message:
|
||||
self.user_fback()
|
||||
self.user_feeback()
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue