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

Pushing latest changes and removing unecessary files

This commit is contained in:
cesar 2022-07-05 11:06:45 -03:00
parent 53895d5bf3
commit 7a84c2ca33
15 changed files with 21 additions and 784 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
.gitignore vendored
View File

@ -1,12 +1,16 @@
launch/debug.launch
launch/mocap_*
launch/cortex_bridge.launch launch/cortex_bridge.launch
src/MoCap_Localization_*.py launch/debug.launch
src/Mocap_*.py launch/klausen_dampen.launch
src/segmented_tether.py launch/mocap_*
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/segmented_tether.py
src/segmented_tether_fast.py
msg/Marker.msg msg/Marker.msg
msg/Markers.msg msg/Markers.msg
*.rviz *.rviz
setup.txt

2
CMakeLists.txt
View File

@ -51,7 +51,7 @@ find_package(catkin REQUIRED COMPONENTS
## Generate messages in the 'msg' folder ## Generate messages in the 'msg' folder
add_message_files( add_message_files(
FILES FILES
tethered_status.msg TetheredStatus.msg
RefSignal.msg RefSignal.msg
EulerAngles.msg EulerAngles.msg
LoadAngles.msg LoadAngles.msg

43
launch/cortex_bridge.launch
View File

@ -1,43 +0,0 @@
<?xml version="1.0"?>
<!--
Launch file to use klausen oscillaton damping ctrl in Gazebo
/-->
<launch>
<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 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_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>

75
launch/klausen_dampen.launch
View File

@ -1,75 +0,0 @@
<?xml version="1.0"?>
<!--
Launch file to use klausen oscillaton damping ctrl in Gazebo
/-->
<launch>
<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="test_type" default="step.py" /-->
<arg name="model" default="spiri_with_tether"/>
<arg name='test' default="none"/>
<group ns="sim">
<rosparam file="$(find oscillation_ctrl)/config/Ctrl_param.yaml" />
</group>
<group ns="status">
<param name="test_type" value="$(arg test)"/>
</group>
<node
pkg="oscillation_ctrl"
type="LinkState.py"
name="linkStates_node"
launch-prefix="xterm -e"
/>
<node
pkg="oscillation_ctrl"
type="wpoint_tracker.py"
name="waypoints_server"
/>
<node
pkg="oscillation_ctrl"
type="ref_signalGen.py"
name="refSignal_node"
/>
<node
pkg="oscillation_ctrl"
type="klausen_control.py"
name="klausenCtrl_node"
output="screen"
/>
<node
pkg="oscillation_ctrl"
type="pathFollow_node"
name="pathFollow_node"
launch-prefix="xterm -e"
/>
<group if="$(eval test != 'none')">
<node
pkg="oscillation_ctrl"
type="perform_test.py"
name="test_node"
launch-prefix="xterm -e"
/>
</group>
<node pkg="geometric_controller" type="geometric_controller_node" name="geometric_controller" output="screen">
<param name="mav_name" type="string" value="$(arg mav_name)" />
<!--remap from="command/bodyrate_command" to="/mavros/setpoint_raw/attitude"/-->
<param name="ctrl_mode" value="$(arg command_input)" />
<param name="max_acc" value="8.0" />
<param name="Kp_x" value="8.0" />
<param name="Kp_y" value="8.0" />
<param name="Kp_z" value="10.0" />
<param name="Kv_x" value="3.0" />
<param name="Kv_y" value="3.0" />
<param name="Kv_z" value="6.0" />
</node>
<!-- PX4 LAUNCH -->
<include file="$(find px4)/launch/$(arg model).launch"/>
</launch>

37
launch/mocap_sim.launch
View File

@ -1,37 +0,0 @@
<?xml version="1.0"?>
<!--
Launch file to use klausen oscillaton damping ctrl in Gazebo
/-->
<launch>
<arg name="mav_name" default="spiri_mocap"/>
<arg name="command_input" default="1" />
<arg name="model" default="headless_spiri_mocap"/>
<node
pkg="oscillation_ctrl"
type="MoCap_Localization_fake.py"
name="fakeMocap_node"
launch-prefix="xterm -e"
/>
<node
pkg="oscillation_ctrl"
type="offb_node"
name="offb_node"
launch-prefix="xterm -e"
/>
<!--node pkg="geometric_controller" type="geometric_controller_node" name="geometric_controller" output="screen">
<param name="mav_name" type="string" value="$(arg mav_name)" />
<!-remap from="command/bodyrate_command" to="/mavros/setpoint_raw/attitude"/->
<param name="ctrl_mode" value="$(arg command_input)" />
<param name="max_acc" value="8.0" />
<param name="Kp_x" value="8.0" />
<param name="Kp_y" value="8.0" />
<param name="Kp_z" value="10.0" />
<param name="Kv_x" value="3.0" />
<param name="Kv_y" value="3.0" />
<param name="Kv_z" value="6.0" />
</node-->
<!-- PX4 LAUNCH -->
<include file="$(find px4)/launch/$(arg model).launch"/>
</launch>

0
msg/tethered_status.msg → msg/TetheredStatus.msg
View File

120
setup.txt
View File

@ -1,120 +0,0 @@
Cesar Rodriguez
February 2022
Steps to recreate stable PX4 environment + working repo
STEP 1) Installing ROS Melodic
- SETUP SOURCES.LIST
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
- SETUP YOUR KEYS
sudo apt install curl # if you haven't already installed curl
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
- INSTALLATION
sudo apt update
sudo apt install ros-melodic-desktop-full
- ENVIRONMENT SETUP
echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
- DEPENDENCIES
sudo apt install python-rosdep python-rosinstall python-rosinstall-generator python-wstool build-essential
Initilize rosdep:
sudo apt install python-rosdep
sudo rosdep init
rosdep update
- PX4 DEPENDENCIES
sudo apt-get install python-catkin-tools python-rosinstall-generator -y
wstool init ~/catkin_ws/src
- MAVLINK
rosinstall_generator --rosdistro melodic mavlink | tee /tmp/mavros.rosinstall
- MAVROS
rosinstall_generator --upstream mavros | tee -a /tmp/mavros.rosinstall
- CREATE WORKSPACE AND DEPS
cd ~/catkin_ws
wstool merge -t src /tmp/mavros.rosinstall
wstool update -t src -j4
rosdep install --from-paths src --ignore-src -y
- INSTALL GEOGRAPHIC LIB DATASETS
cd ~/catkin_ws
sudo ./src/mavros/mavros/scripts/install_geographiclib_datasets.sh
- BUILD SOURCE
cd ~/catkin_ws
catkin build
STEP 2) PX4 Environment Development
- DOWNLOAD PX4 SOURCE CODE
git clone https://github.com/PX4/PX4-Autopilot.git --recursive
- RUN UBUNTU.SH
bash ./PX4-Autopilot/Tools/setup/ubuntu.sh
Restart computer after it is done
- BUILD ROS/GAZEBO: Gets Gazebo9
wget https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim_ros_melodic.sh
bash ubuntu_sim_ros_melodic.sh
- Download QGroundControl from:
https://docs.qgroundcontrol.com/master/en/releases/daily_builds.html
- Build jMAVSim and Gazebo
cd ~/PX4-Autopilot
make px4_sitl jmavsim
%%% May need to open QGroundControl for it to work %%%
make px4_sitl gazebo
- Create px4 package
cd ~/PX4-Autopilot
DONT_RUN=1 make px4_sitl_default gazebo
source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo
roslaunch px4 posix_sitl.launch
STEP 3) This is Cesar stuff, need to do stuff to rebuild oscillation_ctrl
- INSTALL XTERM
sudo apt-get update -y
sudo apt-get install -y xterm
-INSTALL MAVROS_CONTROLLERS
cd ~/catkin_ws/src
clone repo:
git clone https://github.com/Jaeyoung-Lim/mavros_controllers
Download dependencies:
cd ~/catkin_ws
wstool merge -t src src/mavros_controllers/dependencies.rosinstall
wstool update -t src -j4
rosdep install --from-paths src --ignore-src -y --rosdistro $ROS_DISTRO
catkin build
source ~/catkin_ws/devel/setup.bash
~Troubleshooting: https://github.com/Jaeyoung-Lim/mavros_controllers
-TOOLS/SITL_GAZEBO
copy (or add) files in oscillation_ctrl/models and oscillation_ctrl/worlds to PX4-Autopilot/Tools/sitl_gazebo/models and PX4-Autopilot/Tools/sitl_gazebo/worlds respectively
-ROMFS/PX4FMU_COMMON
copy (or add) files in oscillation_ctrl/airframes to PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes
add file name to CmakeLists.txt in same 'airframe' folder (with number)
add airframe name in ~/PX4-Autopilot/platforms/posix/cmake/sitl_target.cmake (no number!)
-LAUNCH FILES
ccopy (or add) files from px4_launch directory to ~/PX4-Autopilot/launch
-MAVROS
- in px4.launch, replace:
<arg name="fcu_url" default="/dev/ttyACM0:57600" />
<arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" />
- with:
<arg name="fcu_url" default="udp://:14551@192.168.1.91:14556" />
<arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" />
- CHANGE DEVEL/SETUP.BASH
In catkin_ws (or any working directory) add to devel/setup.bash:
CURRENT_DIR=$(pwd)
cd ~/PX4-Autopilot
source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo
cd $CURRENT_DIR
JINJA TETHER FILE
- First two elements can be changed to tweak tether parameters
- number_elements: number of segments tether will be composed of
- tl: segment length (should be no shorter than 0.3 meters)

6
src/LinkState.py
View File

@ -8,7 +8,7 @@ import rosservice
import time import time
import math import math
from tf.transformations import * from tf.transformations import *
from oscillation_ctrl.msg import tethered_status, LoadAngles from oscillation_ctrl.msg import TetheredStatus, LoadAngles
from geometry_msgs.msg import Pose from geometry_msgs.msg import Pose
from gazebo_msgs.srv import GetLinkState from gazebo_msgs.srv import GetLinkState
from std_msgs.msg import Bool from std_msgs.msg import Bool
@ -41,7 +41,7 @@ class Main:
self.bool = False self.bool = False
# variables for message gen # variables for message gen
self.status = tethered_status() self.status = TetheredStatus()
self.drone_id = 'spiri_with_tether::spiri::base' self.drone_id = 'spiri_with_tether::spiri::base'
self.pload_id = 'spiri_with_tether::mass::payload' self.pload_id = 'spiri_with_tether::mass::payload'
self.loadAngles = LoadAngles() self.loadAngles = LoadAngles()
@ -64,7 +64,7 @@ class Main:
rospy.wait_for_service(self.service1,timeout=10) rospy.wait_for_service(self.service1,timeout=10)
# publisher(s) # publisher(s)
self.twobody_pub = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1) 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.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_wd = rospy.Publisher('/status/path_follow', Bool, queue_size=1)

261
src/MoCap_Localization_Tether.py
View File

@ -1,261 +0,0 @@
#!/usr/bin/env python2.7
### Cesar Rodriguez Feb 2022
### Script to determine payload and drone state using mocap
import rospy, tf
import rosservice
import time
import math
from tf.transformations import *
from offboard_ex.msg import tethered_status
from geometry_msgs.msg import Pose
from gazebo_msgs.srv import GetLinkState
from std_msgs.msg import Bool
class Main:
def __init__(self):
# rate(s)
rate = 40 # rate for the publisher method, specified in Hz -- 20 Hz
self.dt = 1.0/rate
# Variables needed for testing start
self.tstart = rospy.get_time() # Keep track of the start time
while self.tstart == 0.0: # Need to make sure get_rostime works
self.tstart = rospy.get_time()
### -*-*-*- 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.phi = 0.0 # Payload angle of deflection from x-axis
self.theta = 0.0 # Payload angle of deflection from y-axis
self.tetherL = 0.0 # Tether length
self.has_run = 0 # Boolean to keep track of first run instance
self.phidot = 0.0 #
self.thetadot = 0.0 #
self.phibuf = 0.0 # Need buffers to determine their rates
self.thetabuf = 0.0 #
self.pload = True # Check if payload exists
# Max dot values to prevent 'blowup'
self.phidot_max = 3.0
self.thetadot_max = 3.0
# variables for message gen
self.status = tethered_status()
self.status.drone_id = 'spiri_with_tether::spiri::base'
self.status.drone_pos = Pose()
self.status.pload_id = 'spiri_with_tether::mass::payload'
self.status.pload_pos = Pose()
self.status.phi = 0.0
self.status.theta = 0.0
# service(s)
self.service1 = '/gazebo/get_link_state'
# need service list to check if models have spawned
self.service_list = rosservice.get_service_list()
# wait for service to exist
while self.service1 not in self.service_list:
print "Waiting for models to spawn..."
self.service_list = rosservice.get_service_list()
if rospy.get_time() - self.tstart >= 10.0:
break
# publisher(s)
self.publisher = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1)
self.pub_wd = rospy.Publisher('/status/path_follow', Bool, queue_size=1)
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)
def cutoff(self,value,ceiling):
"""
Takes in value and returns ceiling
if value > ceiling. Otherwise, it returns
value back
"""
# initilize sign
sign = 1
# check if value is negative
if value < 0.0:
sign = -1
# Cutoff value at ceiling
if (value > ceiling or value < -ceiling):
output = sign*ceiling
else:
output = value
return output
def euler_array(self,pose):
"""
Takes in pose msg object and outputs array of euler angs:
q[0] = Roll
q[1] = Pitch
q[2] = Yaw
"""
q = euler_from_quaternion([pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w])
return q
# def FRD_Transform(pose)
# '''
# Transforms mocap reading to proper coordinate frame
# '''
# pose.position.x =
# pose.position.y =
# pose.position.z =
# Keep the w same and change x, y, and z as above.
# pose.orientation.x =
# pose.orientation.y =
# pose.orientation.z =
# pose.orientation.w =
# return pose
# Get link states (drone and pload) and determine angle between them
def link_state(self,pub_timer):
try:
# State which service we are querying
get_P = rospy.ServiceProxy(self.service1,GetLinkState)
# Set reference frame
reference = ''
# Establish links needed --> Spiri base and payload
# P = Position vector
drone_P = get_P(self.status.drone_id,reference)
# Get orientation of drone in euler angles
drone_Eul = self.euler_array(drone_P.link_state.pose)
# Check if payload is part of simulation
if not drone_P.success:
self.status.drone_id = 'spiri::base'
drone_P = get_P(self.status.drone_id,reference) # i.e. no payload
self.pload = False
pload_P = get_P(self.status.pload_id,reference)
if not self.has_run == 1:
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('sim/tether_length',self.tetherL)
else:
self.tetherL = 0
self.has_run = 1
# 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
drone_Pz = drone_P.link_state.pose.position.z
if self.pload == True: # If there is payload, determine the variables
# 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.theta = 0
else:
self.theta = math.asin(x_sep/self.tetherL)
# Determine thetadot
self.thetadot = (self.theta - self.thetabuf)/self.dt
self.thetadot = self.cutoff(self.thetadot,self.thetadot_max)
self.thetabuf = self.theta
# Determine phi (roll)
y_sep = pload_Py - drone_Py
if math.fabs(y_sep) >= self.tetherL or y_sep == 0:
self.phi = 0
else:
self.phi = math.asin(y_sep/self.tetherL)
# Determine phidot
self.phidot = (self.phi - self.phibuf)/self.dt
self.phidot = self.cutoff(self.phidot,self.phidot_max)
self.phibuf = self.phi # Update buffer
else: # Otherwise, vars = 0
x_sep = self.phi = self.phidot = self.theta = self.thetadot = 0
# Print and save results
print "\n"
rospy.loginfo("")
print"Roll: "+str(round(drone_Eul[0],2)),"\nPitch: "+str(round(drone_Eul[1],2)),"\nYaw: "+str(round(drone_Eul[2],2))
print "drone pos.x: " + str(round(drone_Px,2))
print "drone pos.y: " + str(round(drone_Py,2))
print "drone pos.z: " + str(round(drone_Pz,2))
print "phi: " + str(round(self.phi*180/3.14,3))
print "theta: " + str(round(self.theta*180/3.14,3))
# Populate message
self.status.drone_pos = drone_P.link_state.pose
self.status.pload_pos = pload_P.link_state.pose
self.status.length = self.tetherL
self.status.phi = self.phi
self.status.phidot = self.phidot
self.status.theta = self.theta
self.status.thetadot = self.thetadot
# Publish message
self.publisher.publish(self.status)
except rospy.ServiceException as e:
rospy.loginfo("Get Link State call failed: {0}".format(e))
# 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)
if __name__=="__main__":
# Initiate ROS node
rospy.init_node('linkStates_node',anonymous=False)
try:
Main() # create class object
rospy.spin() # loop until shutdown signal
except rospy.ROSInterruptException:
pass

183
src/MoCap_Localization_noTether.py
View File

@ -1,183 +0,0 @@
#!/usr/bin/env python2.7
### Cesar Rodriguez Feb 2022
### Script to determine payload and drone state using mocap
import rospy, tf
import rosservice
import time
import math
from tf.transformations import *
from oscillation_ctrl.msg import tethered_status
from geometry_msgs.msg import PoseStamped, Point
from std_msgs.msg import Bool
class Main:
def __init__(self):
# rate(s)
rate = 120 # rate for the publisher method, specified in Hz -- 20 Hz
# Variables needed for testing start
self.tstart = rospy.get_time() # Keep track of the start time
while self.tstart == 0.0: # Need to make sure get_rostime works
self.tstart = rospy.get_time()
### -*-*-*- Do not need this unless a test is being ran -*-*-*- ###
# How long should we wait before before starting test
#self.param_exists = False
#while self.param_exists == False:
# if rospy.has_param('sim/wait'):
# self.wait = rospy.get_param('sim/wait') # wait time
# self.param_exists = True
# elif rospy.get_time() - self.tstart >= 3.0:
# break
# Will be set to true when test should start
#self.bool = False
### -*-*-*- END -*-*-*- ###
# initialize variables
self.drone_pose = PoseStamped()
self.buff_pose = PoseStamped()
self.eul = [0.0,0.0,0.0]
# Max dot values to prevent 'blowup'
self.phidot_max = 3.0
self.thetadot_max = 3.0
# variables for message gen
# service(s)
# need service list to check if models have spawned
# wait for service to exist
# publisher(s)
### Since there is no tether, we can publish directly to mavros
self.pose_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=1)
self.pub_timer = rospy.Timer(rospy.Duration(1.0/rate), self.publisher)
# subscriber(s)
rospy.Subscriber('/cortex/body_pose', PoseStamped, self.bodyPose_cb)
def cutoff(self,value,ceiling):
"""
Takes in value and returns ceiling
if value > ceiling. Otherwise, it returns
value back
"""
# initilize sign
sign = 1
# check if value is negative
if value < 0.0:
sign = -1
# Cutoff value at ceiling
if (value > ceiling or value < -ceiling):
output = sign*ceiling
else:
output = value
return output
def euler_array(self):
"""
Takes in pose msg object and outputs array of euler angs:
eul[0] = Roll
eul[1] = Pitch
eul[2] = Yaw
"""
self.eul = euler_from_quaternion([self.drone_pose.pose.orientation.x,
self.drone_pose.pose.orientation.y,
self.drone_pose.pose.orientation.z,
self.drone_pose.pose.orientation.w])
self.q = quaternion_from_euler(self.eul[0],self.eul[1],self.eul[2])
offset_yaw = math.pi/2
q_offset = quaternion_from_euler(0,0,-offset_yaw)
self.q = quaternion_multiply(self.q,q_offset)
self.eul = euler_from_quaternion([self.q[0],self.q[1],self.q[2],self.q[3]])
self.drone_pose.pose.orientation.x = self.q[0]
self.drone_pose.pose.orientation.y = self.q[1]
self.drone_pose.pose.orientation.z = self.q[2]
self.drone_pose.pose.orientation.w = self.q[3]
def FRD_Transform(self):
'''
Transforms mocap reading to proper coordinate frame
'''
# self.drone_pose = self.buff_pose
self.drone_pose.header.frame_id = "/map"
# self.drone_pose.pose.position.x = 0
# self.drone_pose.pose.position.y = 0
# self.drone_pose.pose.position.z = 0.5
#Keep the w same and change x, y, and z as above.
# self.drone_pose.pose.orientation.x = 0
# self.drone_pose.pose.orientation.y = 0
# self.drone_pose.pose.orientation.z = 0
# self.drone_pose.pose.orientation.w = 1
self.euler_array() # get euler angles of orientation for user
# self.drone_pose.pose.position.x = self.buff_pose.pose.position.y
# self.drone_pose.pose.position.y = self.buff_pose.pose.position.x
# self.drone_pose.pose.position.z = -self.buff_pose.pose.position.z
# Keep the w same and change x, y, and z as above.
# self.drone_pose.pose.orientation.x = self.buff_pose.pose.orientation.y
# self.drone_pose.pose.orientation.y = self.buff_pose.pose.orientation.x
# self.drone_pose.pose.orientation.z = -self.buff_pose.pose.orientation.z
# self.drone_pose.pose.orientation.w = self.buff_pose.pose.orientation.w
# def path_follow(self,path_timer):
# now = rospy.get_time()
# if now - self.tstart < self.wait:
# self.bool = False
# else:
# self.bool = True
# self.pub_wd.publish(self.bool)
def bodyPose_cb(self,msg):
try:
self.drone_pose = msg
except ValueError:
pass
def publisher(self,pub_timer):
self.FRD_Transform()
self.pose_pub.publish(self.drone_pose)
print "\n"
rospy.loginfo("")
print "drone pos.x: " + str(round(self.drone_pose.pose.position.x,2))
print "drone pos.y: " + str(round(self.drone_pose.pose.position.y,2))
print "drone pos.z: " + str(round(self.drone_pose.pose.position.z,2))
print "Roll: " + str(round(self.eul[0]*180/3.14,2))
print "Pitch: " + str(round(self.eul[1]*180/3.14,2))
print "Yaw: " + str(round(self.eul[2]*180/3.14,2))
if __name__=="__main__":
# Initiate ROS node
rospy.init_node('MoCap_node',anonymous=False)
try:
Main() # create class object
rospy.spin() # loop until shutdown signal
except rospy.ROSInterruptException:
pass

4
src/klausen_control.py
View File

@ -25,7 +25,7 @@ class Main:
def __init__(self): def __init__(self):
# rate(s) # rate(s)
rate = 200 # rate for the publisher method, specified in Hz -- 20 Hz rate = 50 # rate for the publisher method, specified in Hz -- 20 Hz
# initialize variables # initialize variables
@ -116,7 +116,7 @@ class Main:
# Topic, msg type, and class callback method # Topic, msg type, and class callback method
rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb)
rospy.Subscriber('/reference/path', RefSignal, self.refsig_cb) rospy.Subscriber('/reference/path', RefSignal, self.refsig_cb)
#rospy.Subscriber('/status/twoBody_status', tethered_status, self.dronePos_cb) #rospy.Subscriber('/status/twoBody_status', TetheredStatus, self.dronePos_cb)
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb)
rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb)
rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)

8
src/path_follow.cpp
View File

@ -96,13 +96,13 @@ int main(int argc, char **argv)
("status/waypoint_tracker"); ("status/waypoint_tracker");
//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
ros::Rate rate(20.0); ros::Rate rate_wait(20.0);
// 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("Waiting for FCU connection");
rate.sleep(); rate_wait.sleep();
} }
if (current_state.connected){ if (current_state.connected){
@ -113,7 +113,7 @@ int main(int argc, char **argv)
/*********** Initiate variables ************************/ /*********** Initiate variables ************************/
//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
ros::Rate rate_pub(75.0); ros::Rate rate_pub(25.0);
// Retrieve desired waypoints // Retrieve desired waypoints
oscillation_ctrl::WaypointTrack wpoint_srv; oscillation_ctrl::WaypointTrack wpoint_srv;
@ -150,7 +150,7 @@ int main(int argc, char **argv)
local_pos_pub.publish(buff_pose); local_pos_pub.publish(buff_pose);
ros::spinOnce(); ros::spinOnce();
ROS_INFO("Publishing position setpoints"); ROS_INFO("Publishing position setpoints");
rate.sleep(); rate_wait.sleep();
} }
while(ros::ok()){ while(ros::ok()){

48
src/quarternion.py
View File

@ -1,48 +0,0 @@
#!/usr/bin/env python
import rospy, tf
import numpy as np
import time
import math
from tf.transformations import *
from geometry_msgs.msg import PoseStamped, Quaternion
class Main:
def __init__(self):
# rate
rate = 40
self.Euler = [0,0,1.571]
self.pose = PoseStamped()
self.q = [0,0,0,0]
self.pub_q = rospy.Publisher('/quaternions',PoseStamped,queue_size = 10)
self.pub_time = rospy.Timer(rospy.Duration(1.0/rate),self.publisher)
def publisher(self,pub_tim):
q = quaternion_from_euler(self.Euler[0],self.Euler[1],self.Euler[2])
#q_msg = Quaternion(q[0],q[1],q[2],q[3])
self.pose.header.stamp = rospy.Time.now()
self.pose.pose.position.x = 0
self.pose.pose.position.y = 0
self.pose.pose.position.z = 2.5
self.pose.pose.orientation.x = q[0]
self.pose.pose.orientation.y = q[1]
self.pose.pose.orientation.z = q[2]
self.pose.pose.orientation.w = q[3]
self.pub_q.publish(self.pose)
rospy.loginfo("Fly to altitude: %.2f m",self.pose.pose.position.z)
if __name__=="__main__":
rospy.init_node('quart_node')
try:
Main()
rospy.spin()
except rospy.ROSInterruptException:
pass

2
src/ref_signalGen.py
View File

@ -59,7 +59,7 @@ class Main:
# Topic, msg type, and class callback method # Topic, msg type, and class callback method
rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb)
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb)
#rospy.Subscriber('/status/twoBody_status', tethered_status, self.dronePos_cb) #rospy.Subscriber('/status/twoBody_status', TetheredStatus, self.dronePos_cb)
rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb)
rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)

2
src/wpoint_tracker.py
View File

@ -6,7 +6,7 @@
import rospy, tf import rospy, tf
import rosservice import rosservice
import time import time
from geometry_msgs.msg import Point,Pose from geometry_msgs.msg import Point
from oscillation_ctrl.srv import WaypointTrack,WaypointTrackResponse from oscillation_ctrl.srv import WaypointTrack,WaypointTrackResponse
class Main: class Main: