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

Adding mocap files for GPS denied environments

This commit is contained in:
cesar 2022-09-09 18:03:40 -03:00
parent 8fcb292d0c
commit 5b224e599d
15 changed files with 1295 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored
View File

@ -2,13 +2,10 @@ 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}

12
config/Ctrl_param.yaml
View File

@ -1,8 +1,14 @@
# Ros param when using Klausen Ctrl # Ros param when using Klausen Ctrl
wait_time: 30 wait_time: 30
#drone_mass: 0.5841 #drone_mass: 0.614 # weight with new battery
drone_mass: 1.437 drone_mass: 0.602 # weight with old battery
pload_mass: 0.50 #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 use_ctrl: false

4
config/gazebo_config.yaml
View File

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

93
launch/mocap_oscillation_damp.launch Normal file
View File

@ -0,0 +1,93 @@
<?xml version="1.0"?>
<!--
Launch file to use klausen oscillaton damping ctrl in Gazebo
/-->
<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/Ctrl_param.yaml" />
<param name="test_type" value="$(arg test)"/>
</group>
<!-- LOCALIZES DRONE & DETERMINES LOAD ANGLES -->
<node
pkg="oscillation_ctrl"
type="Mocap_Bridge.py"
name="localize_node"
launch-prefix="xterm -e"
/>
<!-- DETERMINES DESIRED POSITION -->
<node
pkg="oscillation_ctrl"
type="wpoint_tracker.py"
name="waypoints_server"
/>
<!-- CREATES DESIRED TRAJECTORY/ REFERENCE SIGNAL -->
<node
pkg="oscillation_ctrl"
type="ref_signalGen.py"
name="refSignal_node"
launch-prefix="xterm -e"
/>
<!-- DETERMINES DESIRED ATTITUDE AND THRUST BASED ON REF. SIG. -->
<node
pkg="oscillation_ctrl"
type="klausen_control.py"
name="klausenCtrl_node"
launch-prefix="xterm -e"
output='screen'
/>
<!-- PUBLISHES DESIRED COMMANDS -->
<node
pkg="oscillation_ctrl"
type="mocap_pathFollow_node"
name="mocap_pathFollow_node"
launch-prefix="xterm -e"
/>
<!-- RUNS DIFFERENT TESTS IF DESIRED -->
<group if="$(eval test != 'none')">
<node
pkg="oscillation_ctrl"
type="perform_test.py"
name="test_node"
launch-prefix="xterm -e"
/>
</group>
<!-- RUNS TEST -->
<node
pkg="oscillation_ctrl"
type="mocap_runTest.py"
name="mocap_Test"
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>

71
launch/mocap_sim.launch Normal file
View File

@ -0,0 +1,71 @@
<?xml version="1.0"?>
<!--
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>
<node
pkg="oscillation_ctrl"
type="MoCap_Localization_fake.py"
name="fakeMocap_node"
launch-prefix="xterm -e"
/>
<!-- DETERMINES DESIRED POSITION -->
<node
pkg="oscillation_ctrl"
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"
type="ref_signalGen.py"
name="refSignal_node"
/>
<!-- DETERMINES DESIRED ATTITUDE AND THRUST BASED ON REF. SIG. -->
<node
pkg="oscillation_ctrl"
type="klausen_control.py"
name="klausenCtrl_node"
launch-prefix="xterm -e"
/>
<!-- PUBLISHES DESIRED COMMANDS -->
<node
pkg="oscillation_ctrl"
type="mocap_pathFollow_node"
name="mocap_pathFollow_node"
launch-prefix="xterm -e"
/>
<!-- RUNS TEST -->
<node
pkg="oscillation_ctrl"
type="mocap_runTest.py"
name="mocap_Test"
launch-prefix="xterm -e"
/>
</group>
<!-- PX4 LAUNCH -->
<include file="$(find px4)/launch/$(arg model).launch"/>
</launch>

65
launch/mocap_takeoff_noCtrl.launch Normal file
View File

@ -0,0 +1,65 @@
<?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>

239
src/MoCap_Localization_fake.py Executable file
View File

@ -0,0 +1,239 @@
#!/usr/bin/env python2.7
### Cesar Rodriguez Mar 2022
### Script to simulate mocap readings and see how PX4 behaves
import rospy, tf
import rosservice
import time
import math
import random
from tf.transformations import *
from oscillation_ctrl.msg import TetheredStatus, LoadAngles
from geometry_msgs.msg import PoseStamped
from gazebo_msgs.srv import GetLinkState
from std_msgs.msg import Bool
class Main:
def __init__(self):
# rate(s)
pub_rate = 50 # rate for the publisher method, specified in Hz -- 20 Hz
loc_rate = 60 # rate we want to localize vehicle -- 60 Hz
self.dt = 1.0/loc_rate
self.user_fback = True
rospy.sleep(5) # Sleep for 5 sec. Need to give time to Gazebo to run
# 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()
# initialize variables
self.tetherL = 0.0 # Tether length
self.has_run = False # Boolean to keep track of first run instance
self.phibuf = 0.0 # Need buffers to determine their rates
self.thetabuf = 0.0 #
self.pload = False # Check if payload exists
# Max dot values to prevent 'blowup'
self.angledot_max = 2.0
self.drone_eul = [0.0,0.0,0.0]
# variables for message gen
#self.buff_pose1 = PoseStamped()
self.drone_pose = PoseStamped()
self.pload_pose = PoseStamped()
self.load_angles = LoadAngles()
self.twobody_status = TetheredStatus()
self.drone_id = 'spiri_with_tether::spiri::base'
self.pload_id = 'spiri_with_tether::mass::payload'
# service(s)
self.service1 = '/gazebo/get_link_state'
self.service2 = '/gazebo/set_link_properties'
# need service list to check if models have spawned
self.service_list = rosservice.get_service_list()
# wait for service to exist
while self.service1 not in self.service_list:
print ("Waiting for models to spawn...")
self.service_list = rosservice.get_service_list()
if rospy.get_time() - self.tstart >= 10.0:
break
# publisher(s)
self.twobody_pub = rospy.Publisher('/status/twoBody_status', TetheredStatus, queue_size=1)
self.loadAng_pub = rospy.Publisher('/status/load_angles', LoadAngles, queue_size=1)
#self.pub_wd = rospy.Publisher('/status/path_follow', Bool, queue_size=1)
#self.pub_timer = rospy.Timer(rospy.Duration(1.0/rate), self.link_state)
#self.path_timer = rospy.Timer(rospy.Duration(40.0/rate), self.path_follow)
### Since there is no tether, we can publish directly to mavros
self.visionPose_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=1)
self.loc_timer = rospy.Timer(rospy.Duration(1.0/loc_rate), self.mocap_localize)
self.pub_timer = rospy.Timer(rospy.Duration(1.0/pub_rate), self.publisher)
# subscriber(s)
def euler_array(self,orientation):
"""
Takes in pose msg object and outputs array of euler angs:
eul[0] = Roll
eul[1] = Pitch
eul[2] = Yaw
"""
eul = euler_from_quaternion([orientation.x,
orientation.y,
orientation.z,
orientation.w])
return eul
def mocap_localize(self,loc_timer):
"""
Uses Gazebo to simulate MoCap
"""
try:
# State which service we are querying
get_P = rospy.ServiceProxy(self.service1,GetLinkState)
# Set reference frame
reference = '' # world ref frame
# Establish links needed --> Spiri base and payload
# P = Position vector
drone_P = get_P(self.drone_id,reference)
# Check if payload is part of simulation
if not drone_P.success:
self.drone_id = 'spiri_mocap::base'
drone_P = get_P(self.drone_id,reference) # i.e. no payload
self.drone_P = drone_P
pload_P = get_P(self.pload_id,reference)
if pload_P.success: self.pload = True
if not self.has_run:
if self.pload == True:
# Get tether length based off initial displacement
self.tetherL = math.sqrt((drone_P.link_state.pose.position.x -
pload_P.link_state.pose.position.x)**2 +
(drone_P.link_state.pose.position.y -
pload_P.link_state.pose.position.y)**2 +
(drone_P.link_state.pose.position.z -
pload_P.link_state.pose.position.z)**2)
rospy.set_param('status/tether_length',self.tetherL)
else:
self.tetherL = 0.0
self.has_run = True
# Need to detemine their location to get angle of deflection
# Drone
drone_Px = drone_P.link_state.pose.position.x
drone_Py = drone_P.link_state.pose.position.y
# Get drone orientation
if self.pload == True: # If there is payload, determine the variables
self.twobody_status.pload = True
# Pload
pload_Px = pload_P.link_state.pose.position.x
pload_Py = pload_P.link_state.pose.position.y
# Determine theta (pitch)
x_sep = pload_Px - drone_Px
if math.fabs(x_sep) >= self.tetherL or x_sep == 0:
self.load_angles.theta = 0
else:
self.load_angles.theta = math.asin(x_sep/self.tetherL)
# Determine thetadot
# self.load_angles.thetadot = min(self.angledot_max,max((self.load_angles.theta - self.thetabuf)/self.dt,-self.angledot_max))
self.load_angles.thetadot = (self.load_angles.theta - self.thetabuf)/self.dt
self.thetabuf = self.load_angles.theta
# Determine phi (roll)
y_sep = pload_Py - drone_Py
if math.fabs(y_sep) >= self.tetherL or y_sep == 0:
self.load_angles.phi = 0
else:
self.load_angles.phi = -math.asin(y_sep/self.tetherL)
# Determine phidot
# self.load_angles.phidot = min(self.angledot_max,max((self.load_angles.phi - self.phibuf)/self.dt,-self.angledot_max))
self.load_angles.phidot = (self.load_angles.phi - self.phibuf)/self.dt
self.phibuf = self.load_angles.phi # Update buffer
# save pload position
self.twobody_status.pload_pos = pload_P.link_state.pose
self.pload_pose.pose = self.twobody_status.pload_pos
else: # Otherwise, vars = 0
x_sep = self.load_angles.phi = self.load_angles.phidot = self.load_angles.theta = self.load_angles.thetadot = 0
# Populate message
#self.status.drone_pos = drone_P.link_state.pose
self.drone_pose.pose = drone_P.link_state.pose
self.twobody_status.drone_pos = drone_P.link_state.pose
except rospy.ServiceException as e:
rospy.loginfo("Get Link State call failed: {0}".format(e))
def add_noise(self):
# self.drone_pose.pose.position.x = self.drone_pose.pose.position.x
# self.drone_pose.pose.position.y = self.drone_pose.pose.position.y
# self.drone_pose.pose.position.z = self.drone_pose.pose.position.z
self.drone_pose.pose.orientation.x = self.drone_pose.pose.orientation.x + random.uniform(0,0.004)
self.drone_pose.pose.orientation.y = self.drone_pose.pose.orientation.y + random.uniform(0,0.004)
self.drone_pose.pose.orientation.z = self.drone_pose.pose.orientation.z + random.uniform(0,0.004)
self.drone_pose.pose.orientation.w = self.drone_pose.pose.orientation.w + random.uniform(0,0.004)
def publisher(self,pub_timer):
# add noise to signal
self.add_noise()
# fill out necesssary fields
self.drone_pose.header.frame_id = "/map"
self.drone_pose.header.stamp = rospy.Time.now()
self.load_angles.header.stamp = rospy.Time.now()
# publish
self.visionPose_pub.publish(self.drone_pose) # publish pose to mavros
self.loadAng_pub.publish(self.load_angles) # publish load angles to controller
self.twobody_pub.publish(self.twobody_status) # actual pose. Redundant but nice to have
# get euler array for user feedback
self.drone_eul = self.euler_array(self.drone_pose.pose.orientation)
self.user_feedback()
def user_feedback(self):
if self.user_fback:
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.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.load_angles.theta*180/3.14,2)))
print("Phi: " + str(round(self.load_angles.phi*180/3.14,2)))
else:
rospy.loginfo_once(self.tetherL)
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

183
src/MoCap_Localization_noTether.py Executable file
View File

@ -0,0 +1,183 @@
#!/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

120
src/klausen_control.py
View File

@ -32,7 +32,7 @@ class Main:
def __init__(self): def __init__(self):
# rate(s) # rate(s)
rate = 25 # rate for the publisher method, specified in Hz -- 50 Hz rate = 25 # rate for the publisher method, specified in Hz -- 50 Hz #25
# initialize variables # initialize variables
@ -55,6 +55,7 @@ class Main:
self.att_targ = AttitudeTarget() # used to send quaternion attitude commands self.att_targ = AttitudeTarget() # used to send quaternion attitude commands
self.load_angles = LoadAngles() self.load_angles = LoadAngles()
self.EulerAng = [0,0,0] # Will find the euler angles, and then convert to q self.EulerAng = [0,0,0] # Will find the euler angles, and then convert to q
self.EulerPose = [0,0,0]
# Service var # Service var
self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack) self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
@ -74,6 +75,8 @@ class Main:
self.param_exists = False self.param_exists = False
self.tetherL = self.get_tether() self.tetherL = self.get_tether()
self.tether = True if self.tetherL > 0.01 else False self.tether = True if self.tetherL > 0.01 else False
# Check if tether was correctly detected
self.tether_check()
# Retrieve drone and payload masses from config file # Retrieve drone and payload masses from config file
[self.drone_m, self.pl_m] = self.get_masses() [self.drone_m, self.pl_m] = self.get_masses()
@ -93,9 +96,11 @@ 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) self.K1 = np.identity(3)#*0.1
self.K2 = np.identity(5) # self.K1 = np.array([[2,-1,0],[-1,2,-1],[0,-1,2]])
self.tune = 0.1 # Tuning parameter 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.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)
@ -106,16 +111,10 @@ class Main:
# gains for thrust PD Controller # gains for thrust PD Controller
#self.Kp = 3.0 #self.Kp = 3.0
#self.Kd = 3 #self.Kd = 3
self.Kp_thrust = 1.5 self.Kp_thrust = 1.5 #3.0 #1.5
self.Kd_thrust = 1 self.Kd_thrust = 1.0 #3.0 # 1.0
self.max_a = 14.2 #TODO self.R = np.empty([3,3]) # rotation matrix
self.max_t = self.tot_m*self.max_a
self.R = np.empty([3,3]) # rotation matrix
self.e3 = np.array([[0],[0],[1]]) self.e3 = np.array([[0],[0],[1]])
self.gravity = np.array([[0],[0],[9.81]])
self.thrust_offset = 0.0 #1.0 # There was found to be a constant offset 0.7
# Get scaling thrust factor, kf # Get scaling thrust factor, kf
self.kf = self.get_kf() self.kf = self.get_kf()
@ -150,9 +149,11 @@ class Main:
while param_exists == False: while param_exists == False:
if rospy.has_param('status/tether_length'): if rospy.has_param('status/tether_length'):
tether_length = rospy.get_param('status/tether_length') # Tether length tether_length = rospy.get_param('status/tether_length') # Tether length
rospy.loginfo('TETHER LENGTH IN CONFG FILE')
param_exists = True param_exists = True
elif rospy.get_time() - self.tstart >= 5.0: elif rospy.get_time() - self.tstart >= 10.0:
tether_length = 0.0 tether_length = 0.0
rospy.loginfo('TETHER LENGTH NOT FOUND IN PARAMS')
break break
return tether_length return tether_length
@ -164,7 +165,7 @@ class Main:
if rospy.has_param('status/drone_mass'): if rospy.has_param('status/drone_mass'):
drone_m = rospy.get_param('status/drone_mass') # wait time drone_m = rospy.get_param('status/drone_mass') # wait time
param_exists = True param_exists = True
elif rospy.get_time() - self.tstart >= 3.0: elif rospy.get_time() - self.tstart >= 5.0:
drone_m = 0.5841 drone_m = 0.5841
rospy.loginfo('DRONE MASS NOT FOUND IN CONFIG FILE') rospy.loginfo('DRONE MASS NOT FOUND IN CONFIG FILE')
break break
@ -173,8 +174,9 @@ class Main:
while param_exists == False: while param_exists == False:
if rospy.has_param('status/pload_mass'): if rospy.has_param('status/pload_mass'):
pl_m = rospy.get_param('status/pload_mass') # wait time pl_m = rospy.get_param('status/pload_mass') # wait time
rospy.loginfo('PLOAD MASS FOUND')
param_exists = True param_exists = True
elif rospy.get_time() - self.tstart >= 3.0: elif rospy.get_time() - self.tstart >= 5.0:
pl_m = 0.0 pl_m = 0.0
rospy.loginfo('PLOAD MASS NOT FOUND IN CONFIG FILE') rospy.loginfo('PLOAD MASS NOT FOUND IN CONFIG FILE')
break break
@ -201,7 +203,7 @@ class Main:
try: try:
self.load_angles = msg self.load_angles = msg
# Populate self.PHI # Populate self.PHI
self.PHI = np.array([[self.load_angles.theta,self.load_angles.thetadot],[self.load_angles.phi,self.load_angles.phidot]]) self.PHI = np.array([[self.load_angles.theta,self.load_angles.phi],[self.load_angles.thetadot,self.load_angles.phidot]])
except ValueError: except ValueError:
pass pass
@ -209,6 +211,7 @@ class Main:
def dronePos_cb(self,msg): def dronePos_cb(self,msg):
try: try:
self.dr_pos = msg.pose self.dr_pos = msg.pose
self.EulerPose = self.convert2eul(self.dr_pos.orientation)
# self.dr_pos = msg.drone_pos # self.dr_pos = msg.drone_pos
except ValueError: except ValueError:
pass pass
@ -236,7 +239,6 @@ class Main:
self.path_pos = np.array([[msg.position.x],[msg.position.y],[msg.position.z]]) self.path_pos = np.array([[msg.position.x],[msg.position.y],[msg.position.z]])
self.path_vel = np.array([[msg.velocity.x],[msg.velocity.y],[msg.velocity.z]]) self.path_vel = np.array([[msg.velocity.x],[msg.velocity.y],[msg.velocity.z]])
self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z]]) #TODO self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z]]) #TODO
# self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z + 9.81]]) #TODO
except ValueError: except ValueError:
pass pass
@ -311,6 +313,37 @@ class Main:
return rot_matrix return rot_matrix
def convert2eul(self,quaternion_orientation):
"""
Convers quaternion in pose message into euler angles
Input
:param Q: orientatiom pose message
Output
:return: Array of euler angles
"""
x = quaternion_orientation.x
y = quaternion_orientation.y
z = quaternion_orientation.z
w = quaternion_orientation.w
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll = math.atan2(t0, t1)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch = math.asin(t2)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
yaw = math.atan2(t3, t4)
euler = [roll,pitch,yaw]
return euler
def determine_throttle(self): def determine_throttle(self):
# thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3) # thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3)
# Taeyoung Lee, Melvin Leok, and N. Harris McClamroch # Taeyoung Lee, Melvin Leok, and N. Harris McClamroch
@ -330,7 +363,11 @@ class Main:
# thrust = thrust_vector[0]*self.R_e3[0] + thrust_vector[1]*self.R_e3[1] + thrust_vector[2]*self.R_e3[2] # thrust = thrust_vector[0]*self.R_e3[0] + thrust_vector[1]*self.R_e3[1] + thrust_vector[2]*self.R_e3[2]
### OR: ### 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.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 # 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))
@ -349,10 +386,7 @@ class Main:
s_theta = math.sin(self.load_angles.theta) s_theta = math.sin(self.load_angles.theta)
s_phi = math.sin(self.load_angles.phi) s_phi = math.sin(self.load_angles.phi)
# Check if tether was correctly detected # Control matrices
self.tether_check()
# Control matrices - this may be better in _init_
M = [[self.tot_m, 0, 0, 0, L*self.pl_m*c_theta], M = [[self.tot_m, 0, 0, 0, L*self.pl_m*c_theta],
[0, self.tot_m, 0, -L*self.pl_m*c_phi*c_theta, L*self.pl_m*s_phi*s_theta], [0, self.tot_m, 0, -L*self.pl_m*c_phi*c_theta, L*self.pl_m*s_phi*s_theta],
[0, 0, self.tot_m, -L*self.pl_m*c_theta*s_phi, -L*self.pl_m*c_phi*s_theta], [0, 0, self.tot_m, -L*self.pl_m*c_theta*s_phi, -L*self.pl_m*c_phi*s_theta],
@ -386,6 +420,7 @@ class Main:
M_c = M[:3,3:5] # M_1:3,4:5 - rows 1 to 3 and columns 4 to 5 M_c = M[:3,3:5] # M_1:3,4:5 - rows 1 to 3 and columns 4 to 5
C_c = C[:3,3:5] C_c = C[:3,3:5]
# Constants from Eq. 49
Ka = -(D_a + C_a + self.K2[3:5,3:5]) Ka = -(D_a + C_a + self.K2[3:5,3:5])
Kb = -G_a + np.dot(self.K2[3:5,3:5],self.PHI[:,1]) - np.dot(M_b,self.path_acc - np.dot(self.K1,self.dr_vel - self.path_vel)) Kb = -G_a + np.dot(self.K2[3:5,3:5],self.PHI[:,1]) - np.dot(M_b,self.path_acc - np.dot(self.K1,self.dr_vel - self.path_vel))
@ -398,7 +433,7 @@ class Main:
self.a45_buff = odeint(self.statespace,self.a45_0,self.t,args=(Ka,Kb,M_aI)) self.a45_buff = odeint(self.statespace,self.a45_0,self.t,args=(Ka,Kb,M_aI))
# Update a45_0 to new a45. Need to transpose to column vector # Update a45_0 to new a45. Need to transpose to column vector
self.a45_0 = self.a45_buff[1,:] self.a45_0 = self.a45_buff[-1,:]
self.a45 = np.array([[self.a45_0[0]],[self.a45_0[1]]]) self.a45 = np.array([[self.a45_0[0]],[self.a45_0[1]]])
# Get alphadot_4:5 # Get alphadot_4:5
@ -419,38 +454,37 @@ class Main:
self.z2 = g - self.alpha self.z2 = g - self.alpha
B = np.dot(C_c,self.a45) + np.dot(M_c,self.a45dot) B = np.dot(C_c,self.a45) + np.dot(M_c,self.a45dot)
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 # Desired body-oriented forces
# shouldnt it be tot_m*path_acc? # 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*self.path_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
dr_orientation = [self.dr_pos.orientation.x, self.dr_pos.orientation.y, self.dr_pos.orientation.z, self.dr_pos.orientation.w] Fd_tf = Fd
dr_orientation_inv = quaternion_inverse(dr_orientation)
Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],Fd[2],0.0],dr_orientation_inv)) # Real part of Fd needs = 0 Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],Fd[2],0.0],dr_orientation_inv)) # Real part of Fd needs = 0
# Fd_tf = [Fd[0],Fd[1],Fd[2]]
# Convert forces to attiude *EulerAng[2] = yaw = 0 # Convert forces to attiude *EulerAng[2] = yaw = 0
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]) # rospy.loginfo('Fd before transform: %.2f, %.2f, %.2f' % Fd[0],Fd[1],Fd[2])
# rospy.loginfo('Fd before transform: %.2f, %.2f, %.2f', Fd[0], Fd[1], Fd[2])
# rospy.loginfo('Fd after transform: %.2f, %.2f, %.2f', Fd_tf[0],Fd_tf[1],Fd_tf[2])
# Convert Euler angles to quaternions
# for i,val in enumerate(self.EulerAng):
# if val*-1 < 0.0:
# sign = 1
# else:
# sign = -1
# if abs(val) >=0.44:
# self.EulerAng[i] = 0.44*sign
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)
# Populate msg variable # Populate msg variable
# Attitude control # Attitude control
self.att_targ.header.stamp = rospy.Time.now() self.att_targ.header.stamp = rospy.Time.now()
@ -461,15 +495,17 @@ 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): def user_fback(self,F_noTransform, F_Transform):
print('\n')
rospy.loginfo('thrust: %.6f' % self.att_targ.thrust) rospy.loginfo('thrust: %.6f' % self.att_targ.thrust)
rospy.loginfo('\nroll: %.2f\npitch: %.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)
rospy.loginfo('Fd before transform: %.2f, %.2f, %.2f', F_noTransform[0], F_noTransform[1], F_noTransform[2])
rospy.loginfo('Fd after transform: %.2f, %.2f, %.2f', F_Transform[0],F_Transform[1],F_Transform[2])
def publisher(self,pub_time): def publisher(self,pub_time):
self.determine_q() self.determine_q()
self.determine_throttle() self.determine_throttle()
self.pub_att_targ.publish(self.att_targ) self.pub_att_targ.publish(self.att_targ)
self.user_fback()
# --------------------------------------------------------------------------------# # --------------------------------------------------------------------------------#
# ALGORITHM ENDS # ALGORITHM ENDS

194
src/mocap_offb_node.cpp Normal file
View File

@ -0,0 +1,194 @@
/**
* @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;
}

215
src/mocap_path_follow.cpp Normal file
View File

@ -0,0 +1,215 @@
/**
* @file path_follow.cpp
* @brief Offboard path trajectory tracking
*/
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <oscillation_ctrl/RefSignal.h>
#include <oscillation_ctrl/EulerAngles.h>
#include <oscillation_ctrl/WaypointTrack.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 <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <iostream>
#include <cmath> // for std::abs
/********* CALLBACK FUNCTIONS **********************/
// Callback function which will save the current state of the autopilot.
// Allows to check connection, arming, and Offboard tags*/
mavros_msgs::State current_state;
bool land = false;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}
// Cb to determine attitude target
mavros_msgs::AttitudeTarget att_targ;
void att_targ_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg){
att_targ = *msg;
}
// Cb to recieve pose information
geometry_msgs::PoseStamped buff_pose;
geometry_msgs::Quaternion q_init;
geometry_msgs::PoseStamped mavPose;
bool gps_read = false;
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 = 0.0;
buff_pose.pose.orientation.y = 0.0;
buff_pose.pose.orientation.z = 0.0;
buff_pose.pose.orientation.w = 1.0;
gps_read = true;
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "path_follow");
ros::NodeHandle nh;
/********************** SUBSCRIBERS **********************/
// Get current state
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
// Get attitude target from klausen control
ros::Subscriber att_target_sub = nh.subscribe<mavros_msgs::AttitudeTarget>
("command/att_target",10,att_targ_cb);
// Pose subscriber
ros::Subscriber mavPose_sub = nh.subscribe<geometry_msgs::PoseStamped>
("mavros/local_position/pose",10,mavPose_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 attitude and thrust commands
ros::Publisher att_targ_pub = nh.advertise<mavros_msgs::AttitudeTarget>
("mavros/setpoint_raw/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_wait(20.0);
// wait for FCU connection
while(ros::ok() && !current_state.connected){
ros::spinOnce();
ROS_INFO("Waiting for FCU connection");
rate_wait.sleep();
}
if (current_state.connected){
ROS_INFO("Connected to FCU");
} else {
ROS_INFO("Never Connected");
}
/*********** Initiate variables ************************/
//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
ros::Rate rate_pub(25.0);
// Desired mode is offboard
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
// Arm UAV
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
// Take off command
bool takeoff = false, att_cmds = false;
bool oscillation_damp = true;
// Keep track of time since requests
ros::Time tkoff_req = ros::Time::now();
ros::Time last_request = ros::Time::now();
//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(buff_pose);
ros::spinOnce();
ROS_INFO("Publishing position setpoints");
rate_wait.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 - this is only for original hover as
// a change of waypoints is handled by ref_signal.py
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;
} else {
ROS_INFO("NO waypoints received");
}
double alt_des = wpoint_srv.response.xd.z; // Desired height
while(ros::ok()){
if(current_state.mode == "AUTO.LAND"){
land = true;
while(land == true){
ROS_INFO("Des Altitude: LANDING");
}
} else {
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){
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
}
if(current_state.mode == "OFFBOARD" && current_state.armed){
ROS_INFO_ONCE("Spiri is taking off");
if(!takeoff){
tkoff_req = ros::Time::now();
takeoff = true;
}
}
// 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){
ROS_INFO("USING ATTITUDE CTRL");
att_targ.header.stamp = ros::Time::now();
att_targ_pub.publish(att_targ);
} else {
// Check if waypoints have changed
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;
}
local_pos_pub.publish(buff_pose);
ROS_INFO("USING POSITION CTRL");
}
}
ROS_INFO("Des Altitude: %.2f", alt_des);
ROS_INFO("Cur Altitude: %.2f", current_altitude);
ROS_INFO("---------------------------");
ros::spinOnce();
rate_pub.sleep();
}
}
return 0;
}

87
src/mocap_runTest.py Executable file
View File

@ -0,0 +1,87 @@
#! /usr/bin/env python2.7
# Cesar Rodriguez Aug 2022
# Sets attitude mode as well as new waypoints
import rospy
import time
from oscillation_ctrl.srv import WaypointTrack, WaypointTrackResponse
from geometry_msgs.msg import Point
class Main:
def __init__(self):
# initialize variables
self.tstart = rospy.get_time() # Keep track of the start time
while self.tstart == 0.0: # Need to make sure get_rostime works
self.tstart = rospy.get_time()
# set up client
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
# Determine if we want to run test with or without controller
self.change_mode = True # True = Change to oscillation damping mode after wait time
if self.change_mode: self.loginfo_string = 'Attitude mode in...'
else: self.loginfo_string = 'Staying in Position mode.'
self.get_wait_time() # get wait time
self.run_test() # runs the test
def get_wait_time(self):
""" Determine desired wait time based on ros params"""
self.param_exists = False
while self.param_exists == False:
rospy.loginfo_once('Getting wait time')
if rospy.has_param('status/wait_time'):
self.wait_time = rospy.get_param('status/wait_time') # Tether length
self.param_exists = True
rospy.loginfo('Wait time: %.2f',self.wait_time)
elif rospy.get_time() - self.tstart >= 30:
break
def run_test(self):
""" Waits desired amount before setting UAV to appropriate mode, and then sets up desired waypoints"""
run_test = False
use_ctrl = 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:
rospy.loginfo(self.loginfo_string + ' %.2f',time_left)
elif rospy.get_time() - self.tstart >= self.wait_time and not use_ctrl:
rospy.set_param('status/use_ctrl',self.change_mode)
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)
run_test = True
break
def set_waypoint(self,xd):
""" Set waypoints for oscillation controller """
rospy.wait_for_service('/status/waypoint_tracker')
try:
self.get_xd(True,xd)
except ValueError:
pass
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/path_follow.cpp
View File

@ -49,11 +49,11 @@ void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
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.y = q_init.y;
buff_pose.pose.orientation.z = q_init.z;
buff_pose.pose.orientation.w = q_init.w;
gps_read = true; gps_read = true;
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;
} }
} }
} }
@ -101,7 +101,7 @@ int main(int argc, char **argv)
// wait for FCU connection // wait for FCU connection
while(ros::ok() && !current_state.connected){ while(ros::ok() && !current_state.connected){
ros::spinOnce(); ros::spinOnce();
ROS_INFO("Waiting for FCU connection"); ROS_INFO_ONCE("Waiting for FCU connection");
rate_wait.sleep(); rate_wait.sleep();
} }
@ -123,12 +123,10 @@ int main(int argc, char **argv)
mavros_msgs::CommandBool arm_cmd; mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true; arm_cmd.request.value = true;
// Take off command // Boolean to set vehicle into oscillation damp mode
bool takeoff = false, att_cmds = false; bool oscillation_damp = false;
bool oscillation_damp = true;
// Keep track of time since requests // Keep track of time since requests
ros::Time tkoff_req = ros::Time::now();
ros::Time last_request = ros::Time::now(); ros::Time last_request = ros::Time::now();
//send a few setpoints before starting //send a few setpoints before starting
@ -157,6 +155,7 @@ int main(int argc, char **argv)
while(ros::ok()){ while(ros::ok()){
if(gps_read == true){ 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 {
@ -174,10 +173,6 @@ int main(int argc, char **argv)
if(current_state.mode == "OFFBOARD" && current_state.armed){ if(current_state.mode == "OFFBOARD" && current_state.armed){
ROS_INFO_ONCE("Spiri is taking off"); ROS_INFO_ONCE("Spiri is taking off");
if(!takeoff){
tkoff_req = ros::Time::now();
takeoff = true;
}
} }
// Check if we want to use oscillation controller // Check if we want to use oscillation controller
@ -186,30 +181,25 @@ int main(int argc, char **argv)
ros::param::get("/status/use_ctrl", oscillation_damp); ros::param::get("/status/use_ctrl", oscillation_damp);
if(oscillation_damp == true){ if(oscillation_damp == true){
ROS_INFO("ATTITUDE CTRL"); ROS_INFO("ATTITUDE CTRL");
att_targ.header.stamp = ros::Time::now();
// 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");
// Publish position setpoints
local_pos_pub.publish(buff_pose);
} }
} }
// Determine which messages to send
// if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff && oscillation_damp){
// att_targ.header.stamp = ros::Time::now();
// att_targ_pub.publish(att_targ);
// att_cmds = true;
if(oscillation_damp){
att_targ.header.stamp = ros::Time::now();
att_targ_pub.publish(att_targ);
att_cmds = true;
} else {
// Check if waypoints have changed
if (waypoint_cl.call(wpoint_srv))
{
// populate desired waypoints - this is only for original hover as
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;
}
local_pos_pub.publish(buff_pose);
ROS_INFO("POSITION CTRL");
}
ROS_INFO("Des Altitude: %.2f", alt_des); 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("---------------------------");

58
src/ref_signalGen.py
View File

@ -24,10 +24,10 @@ class DesiredPoint():
class Main: class Main:
def __init__(self): def __init__(self):
# rate(s) # rate(s)
rate = 50 # rate for the publisher method, specified in Hz -- 10 Hz rate = 10 # rate for the publisher method, specified in Hz -- 10 Hz
# initialize variables # initialize variables
self.tstart = rospy.get_time() # Keep track of the start time self.tstart = rospy.get_time() # Keep track of the start time
@ -53,7 +53,7 @@ class Main:
self.tetherL = self.get_tether() self.tetherL = self.get_tether()
# --------------------------------------------------------------------------------# # --------------------------------------------------------------------------------#
# SUBSCRIBERS # # SUBSCRIBERS #
# --------------------------------------------------------------------------------# # --------------------------------------------------------------------------------#
# 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)
@ -82,14 +82,15 @@ 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 # 1 also works well :) 3.13 works well? ######################################################################### self.w_tune = 1 # also works well :) 3.13 works well? #########################################################################
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:
if self.tetherL == 0.0: if self.tetherL == 0.0:
self.wn = self.w_tune self.wn = self.w_tune
else: else:
self.wn = math.sqrt(9.81/self.tetherL) self.wn = math.sqrt(9.81/self.tetherL)
# self.wn = 7
self.wd = self.wn*math.sqrt(1 - self.epsilon**2) self.wd = self.wn*math.sqrt(1 - self.epsilon**2)
self.k4 = 4*self.epsilon*self.w_tune self.k4 = 4*self.epsilon*self.w_tune
@ -98,7 +99,7 @@ class Main:
self.k1 = (self.w_tune**4)/(self.k2*self.k3*self.k4) self.k1 = (self.w_tune**4)/(self.k2*self.k3*self.k4)
# For saturation: # For saturation:
self.max = [0, 8, 1.5, 3] #[0, 10, 1.5, 3] self.max = [0, 3, 1.5, 3] #[0, 5, 1.5, 3]
self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack) self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
self.empty_point = Point() # Needed to query waypoint_server self.empty_point = Point() # Needed to query waypoint_server
@ -141,7 +142,7 @@ class Main:
self.at = 3 # acceleration theshold self.at = 3 # acceleration theshold
self.pc = 0.7 # From Klausen 2017 self.pc = 0.7 # From Klausen 2017
self.sk = self.SF_delay_x.shape[1] # from Klausen 2017 self.sk = self.SF_delay_x.shape[1] # from Klausen 2017
self.ak = np.zeros([self.sk]) self.ak = np.zeros(self.sk)
self.s_gain = 0.0 # Gain found from sigmoid self.s_gain = 0.0 # Gain found from sigmoid
self.service_list = rosservice.get_service_list() self.service_list = rosservice.get_service_list()
@ -217,7 +218,7 @@ class Main:
pos,vel,acc,jer = y pos,vel,acc,jer = y
error = xd - pos error = xd - pos
if abs(error) <= 0.25: error = 0.0 if abs(error) <= 0.01: error = 0.0
# Derivative of statesape array: # Derivative of statesape array:
dydt = [vel, acc, jer, dydt = [vel, acc, jer,
@ -304,25 +305,25 @@ class Main:
# SOLVE ODE (get ref pos, vel, accel) # SOLVE ODE (get ref pos, vel, accel)
self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd.x,)) self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd.x,))
self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd.y,)) self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd.y,))
#self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd.z,)) self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd.z,))
for i in range(1,len(self.y0)): for i in range(1,len(self.y0)):
self.x[:,i] = np.clip(self.x[:,i], a_min = -self.max[i], a_max = self.max[i]) self.x[:,i] = np.clip(self.x[:,i], a_min = -self.max[i], a_max = self.max[i])
self.y[:,i] = np.clip(self.y[:,i], a_min = -self.max[i], a_max = self.max[i]) self.y[:,i] = np.clip(self.y[:,i], a_min = -self.max[i], a_max = self.max[i])
#self.z[:,i] = np.clip(self.z[:,i], a_min = -self.max[i], a_max = self.max[i]) self.z[:,i] = np.clip(self.z[:,i], a_min = -self.max[i], a_max = self.max[i])
for j in range(3): # 3 is due to pos, vel, acc. NOT due x, y, z for j in range(3): # 3 is due to pos, vel, acc. NOT due x, y, z
self.delay(j,SHAPEFIL) # Determine the delay (shapefilter) array self.delay(j,SHAPEFIL) # Determine the delay (shapefilter) array
self.EPS_I[3*j+2] = self.z[-1,j] # No need to convolute z-dim
if self.SF_idx < len(self.SF_delay_x[0]): if self.SF_idx < len(self.SF_delay_x[0]):
self.EPS_I[3*j] = self.x[-1,j] self.EPS_I[3*j] = self.x[-1,j]
self.EPS_I[3*j+1] = self.y[-1,j] self.EPS_I[3*j+1] = self.y[-1,j]
#self.EPS_I[3*j+2] = self.z[1,j]
else: else:
self.EPS_I[3*j] = self.A1*self.x[-1,j] + self.A2*self.SF_delay_x[j,0] # Determine convolution (x) self.EPS_I[3*j] = self.A1*self.x[-1,j] + self.A2*self.SF_delay_x[j,0] # Determine convolution (x)
self.EPS_I[3*j+1] = self.A1*self.y[-1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y) self.EPS_I[3*j+1] = self.A1*self.y[-1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y)
#self.EPS_I[3*j+2] = self.z[1,j] # No need to convolute z-dim
self.delay(1,FEEDBACK) # Detemine feedback array self.delay(1,FEEDBACK) # Detemine feedback array
@ -345,18 +346,19 @@ class Main:
self.ref_sig.acceleration.y = self.EPS_F[7] self.ref_sig.acceleration.y = self.EPS_F[7]
# Do not need to evaluate z # Do not need to evaluate z
self.ref_sig.position.z = self.xd.z # self.ref_sig.position.z = self.xd.z
self.ref_sig.velocity.z = 0.0 # self.ref_sig.velocity.z = 0.0
self.ref_sig.acceleration.z = 0.0 # self.ref_sig.acceleration.z = 0.0
#self.ref_sig.position.z = self.EPS_F[2] self.ref_sig.position.z = self.EPS_F[2]
#self.ref_sig.velocity.z = self.EPS_F[5] 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.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.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.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.SF_idx += 1 self.SF_idx += 1
self.FB_idx += 1 self.FB_idx += 1
@ -377,15 +379,15 @@ class Main:
return EPS_D return EPS_D
def screen_output(self): def user_fback(self):
# Feedback to user # Feedback to user
# rospy.loginfo(' Var | x | y | z ') rospy.loginfo(' Var | x | y | z ')
# rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2]) rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2])
# 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_once('Tether length: %.2f',self.tetherL)
# rospy.loginfo('Theta: %.2f',self.load_angles.theta) # rospy.loginfo('Theta: %.2f',self.load_angles.theta)
# rospy.loginfo('Phi: %.2f',self.load_angles.phi) # rospy.loginfo('Phi: %.2f',self.load_angles.phi)
@ -398,7 +400,7 @@ class Main:
self.pub_path.publish(self.ref_sig) self.pub_path.publish(self.ref_sig)
# Give user feedback on published message: # Give user feedback on published message:
self.screen_output() self.user_fback()
if __name__=="__main__": if __name__=="__main__":