Adding mocap files for GPS denied environments
This commit is contained in:
parent
8fcb292d0c
commit
5b224e599d
|
|
@ -2,13 +2,10 @@ 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}
|
||||
|
|
|
|||
|
|
@ -1,8 +1,14 @@
|
|||
# Ros param when using Klausen Ctrl
|
||||
wait_time: 30
|
||||
#drone_mass: 0.5841
|
||||
drone_mass: 1.437
|
||||
pload_mass: 0.50
|
||||
#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,4 +1,6 @@
|
|||
# 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_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,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>
|
||||
|
|
@ -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>
|
||||
|
||||
|
|
@ -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>
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -32,7 +32,7 @@ class Main:
|
|||
def __init__(self):
|
||||
|
||||
# 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
|
||||
|
||||
|
|
@ -55,6 +55,7 @@ class Main:
|
|||
self.att_targ = AttitudeTarget() # used to send quaternion attitude commands
|
||||
self.load_angles = LoadAngles()
|
||||
self.EulerAng = [0,0,0] # Will find the euler angles, and then convert to q
|
||||
self.EulerPose = [0,0,0]
|
||||
|
||||
# Service var
|
||||
self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
|
||||
|
|
@ -74,6 +75,8 @@ class Main:
|
|||
self.param_exists = False
|
||||
self.tetherL = self.get_tether()
|
||||
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
|
||||
[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
|
||||
|
||||
# Tuning gains
|
||||
self.K1 = np.identity(3)
|
||||
self.K2 = np.identity(5)
|
||||
self.tune = 0.1 # Tuning parameter
|
||||
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])
|
||||
# Gain terms
|
||||
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
|
||||
#self.Kp = 3.0
|
||||
#self.Kd = 3
|
||||
self.Kp_thrust = 1.5
|
||||
self.Kd_thrust = 1
|
||||
self.max_a = 14.2 #TODO
|
||||
self.max_t = self.tot_m*self.max_a
|
||||
self.R = np.empty([3,3]) # rotation matrix
|
||||
self.Kp_thrust = 1.5 #3.0 #1.5
|
||||
self.Kd_thrust = 1.0 #3.0 # 1.0
|
||||
self.R = np.empty([3,3]) # rotation matrix
|
||||
self.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
|
||||
self.kf = self.get_kf()
|
||||
|
||||
|
|
@ -150,9 +149,11 @@ class Main:
|
|||
while param_exists == False:
|
||||
if rospy.has_param('status/tether_length'):
|
||||
tether_length = rospy.get_param('status/tether_length') # Tether length
|
||||
rospy.loginfo('TETHER LENGTH IN CONFG FILE')
|
||||
param_exists = True
|
||||
elif rospy.get_time() - self.tstart >= 5.0:
|
||||
elif rospy.get_time() - self.tstart >= 10.0:
|
||||
tether_length = 0.0
|
||||
rospy.loginfo('TETHER LENGTH NOT FOUND IN PARAMS')
|
||||
break
|
||||
return tether_length
|
||||
|
||||
|
|
@ -164,7 +165,7 @@ class Main:
|
|||
if rospy.has_param('status/drone_mass'):
|
||||
drone_m = rospy.get_param('status/drone_mass') # wait time
|
||||
param_exists = True
|
||||
elif rospy.get_time() - self.tstart >= 3.0:
|
||||
elif rospy.get_time() - self.tstart >= 5.0:
|
||||
drone_m = 0.5841
|
||||
rospy.loginfo('DRONE MASS NOT FOUND IN CONFIG FILE')
|
||||
break
|
||||
|
|
@ -173,8 +174,9 @@ class Main:
|
|||
while param_exists == False:
|
||||
if rospy.has_param('status/pload_mass'):
|
||||
pl_m = rospy.get_param('status/pload_mass') # wait time
|
||||
rospy.loginfo('PLOAD MASS FOUND')
|
||||
param_exists = True
|
||||
elif rospy.get_time() - self.tstart >= 3.0:
|
||||
elif rospy.get_time() - self.tstart >= 5.0:
|
||||
pl_m = 0.0
|
||||
rospy.loginfo('PLOAD MASS NOT FOUND IN CONFIG FILE')
|
||||
break
|
||||
|
|
@ -201,7 +203,7 @@ class Main:
|
|||
try:
|
||||
self.load_angles = msg
|
||||
# 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:
|
||||
pass
|
||||
|
||||
|
|
@ -209,6 +211,7 @@ class Main:
|
|||
def dronePos_cb(self,msg):
|
||||
try:
|
||||
self.dr_pos = msg.pose
|
||||
self.EulerPose = self.convert2eul(self.dr_pos.orientation)
|
||||
# self.dr_pos = msg.drone_pos
|
||||
except ValueError:
|
||||
pass
|
||||
|
|
@ -236,7 +239,6 @@ class Main:
|
|||
self.path_pos = np.array([[msg.position.x],[msg.position.y],[msg.position.z]])
|
||||
self.path_vel = np.array([[msg.velocity.x],[msg.velocity.y],[msg.velocity.z]])
|
||||
self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z]]) #TODO
|
||||
# self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z + 9.81]]) #TODO
|
||||
|
||||
except ValueError:
|
||||
pass
|
||||
|
|
@ -311,6 +313,37 @@ class Main:
|
|||
|
||||
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):
|
||||
# thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3)
|
||||
# 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]
|
||||
### 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.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))
|
||||
|
|
@ -349,10 +386,7 @@ class Main:
|
|||
s_theta = math.sin(self.load_angles.theta)
|
||||
s_phi = math.sin(self.load_angles.phi)
|
||||
|
||||
# Check if tether was correctly detected
|
||||
self.tether_check()
|
||||
|
||||
# Control matrices - this may be better in _init_
|
||||
# Control matrices
|
||||
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, 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
|
||||
C_c = C[:3,3:5]
|
||||
|
||||
# Constants from Eq. 49
|
||||
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))
|
||||
|
||||
|
|
@ -398,7 +433,7 @@ class Main:
|
|||
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
|
||||
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]]])
|
||||
|
||||
# Get alphadot_4:5
|
||||
|
|
@ -420,37 +455,36 @@ class Main:
|
|||
|
||||
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
|
||||
# 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.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"
|
||||
self.z1_p = self.z1
|
||||
|
||||
# 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]
|
||||
dr_orientation_inv = quaternion_inverse(dr_orientation)
|
||||
Fd_tf = Fd
|
||||
|
||||
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
|
||||
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])
|
||||
# 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])
|
||||
|
||||
self.user_fback(Fd,Fd_tf)
|
||||
|
||||
# Populate msg variable
|
||||
# Attitude control
|
||||
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.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('\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):
|
||||
self.determine_q()
|
||||
self.determine_throttle()
|
||||
self.pub_att_targ.publish(self.att_targ)
|
||||
self.user_fback()
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
# ALGORITHM ENDS
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -49,11 +49,11 @@ void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
|
|||
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;
|
||||
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
|
||||
while(ros::ok() && !current_state.connected){
|
||||
ros::spinOnce();
|
||||
ROS_INFO("Waiting for FCU connection");
|
||||
ROS_INFO_ONCE("Waiting for FCU connection");
|
||||
rate_wait.sleep();
|
||||
}
|
||||
|
||||
|
|
@ -123,12 +123,10 @@ int main(int argc, char **argv)
|
|||
mavros_msgs::CommandBool arm_cmd;
|
||||
arm_cmd.request.value = true;
|
||||
|
||||
// Take off command
|
||||
bool takeoff = false, att_cmds = false;
|
||||
bool oscillation_damp = true;
|
||||
// Boolean to set vehicle into oscillation damp mode
|
||||
bool oscillation_damp = false;
|
||||
|
||||
// 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
|
||||
|
|
@ -157,6 +155,7 @@ int main(int argc, char **argv)
|
|||
|
||||
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 {
|
||||
|
|
@ -174,10 +173,6 @@ int main(int argc, char **argv)
|
|||
|
||||
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
|
||||
|
|
@ -186,30 +181,25 @@ int main(int argc, char **argv)
|
|||
ros::param::get("/status/use_ctrl", oscillation_damp);
|
||||
if(oscillation_damp == true){
|
||||
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("Cur Altitude: %.2f", current_altitude);
|
||||
ROS_INFO("---------------------------");
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@ class Main:
|
|||
def __init__(self):
|
||||
|
||||
# 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
|
||||
self.tstart = rospy.get_time() # Keep track of the start time
|
||||
|
|
@ -53,7 +53,7 @@ class Main:
|
|||
|
||||
self.tetherL = self.get_tether()
|
||||
# --------------------------------------------------------------------------------#
|
||||
# SUBSCRIBERS #
|
||||
# SUBSCRIBERS #
|
||||
# --------------------------------------------------------------------------------#
|
||||
# Topic, msg type, and class callback method
|
||||
rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb)
|
||||
|
|
@ -82,14 +82,15 @@ class Main:
|
|||
self.EPS_I = np.zeros(9) # Epsilon shapefilter
|
||||
|
||||
# Constants for smooth path generation
|
||||
self.w_tune = 1 # 1 also works well :) 3.13 works well? #########################################################################
|
||||
self.epsilon = 0.7 # Damping ratio
|
||||
self.w_tune = 1 # also works well :) 3.13 works well? #########################################################################
|
||||
self.epsilon = 0.7 # Damping ratio
|
||||
|
||||
# need exception if we do not have tether:
|
||||
if self.tetherL == 0.0:
|
||||
self.wn = self.w_tune
|
||||
else:
|
||||
self.wn = math.sqrt(9.81/self.tetherL)
|
||||
# self.wn = 7
|
||||
|
||||
self.wd = self.wn*math.sqrt(1 - self.epsilon**2)
|
||||
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)
|
||||
|
||||
# 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.empty_point = Point() # Needed to query waypoint_server
|
||||
|
|
@ -141,7 +142,7 @@ class Main:
|
|||
self.at = 3 # acceleration theshold
|
||||
self.pc = 0.7 # 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.service_list = rosservice.get_service_list()
|
||||
|
|
@ -217,7 +218,7 @@ class Main:
|
|||
pos,vel,acc,jer = y
|
||||
|
||||
error = xd - pos
|
||||
if abs(error) <= 0.25: error = 0.0
|
||||
if abs(error) <= 0.01: error = 0.0
|
||||
|
||||
# Derivative of statesape array:
|
||||
dydt = [vel, acc, jer,
|
||||
|
|
@ -304,25 +305,25 @@ class Main:
|
|||
# SOLVE ODE (get ref pos, vel, accel)
|
||||
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.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)):
|
||||
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.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
|
||||
|
||||
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]):
|
||||
self.EPS_I[3*j] = self.x[-1,j]
|
||||
self.EPS_I[3*j+1] = self.y[-1,j]
|
||||
#self.EPS_I[3*j+2] = self.z[1,j]
|
||||
|
||||
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+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
|
||||
|
||||
|
|
@ -345,18 +346,19 @@ class Main:
|
|||
self.ref_sig.acceleration.y = self.EPS_F[7]
|
||||
|
||||
# Do not need to evaluate z
|
||||
self.ref_sig.position.z = self.xd.z
|
||||
self.ref_sig.velocity.z = 0.0
|
||||
self.ref_sig.acceleration.z = 0.0
|
||||
#self.ref_sig.position.z = self.EPS_F[2]
|
||||
#self.ref_sig.velocity.z = self.EPS_F[5]
|
||||
#self.ref_sig.acceleration.z = self.EPS_F[8]
|
||||
# self.ref_sig.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]]
|
||||
|
||||
self.SF_idx += 1
|
||||
self.FB_idx += 1
|
||||
|
|
@ -377,15 +379,15 @@ class Main:
|
|||
|
||||
return EPS_D
|
||||
|
||||
def screen_output(self):
|
||||
def user_fback(self):
|
||||
|
||||
# Feedback to user
|
||||
# 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('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(' Var | x | y | z ')
|
||||
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('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)
|
||||
|
||||
|
|
@ -398,7 +400,7 @@ class Main:
|
|||
self.pub_path.publish(self.ref_sig)
|
||||
|
||||
# Give user feedback on published message:
|
||||
self.screen_output()
|
||||
self.user_fback()
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue