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

Fixing conflicts

This commit is contained in:
cesar 2022-09-16 14:20:10 -03:00
parent 6fe911a64a 3cec1e10f8
commit f39d7c993d
10 changed files with 76 additions and 954 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -9,6 +9,8 @@ src/land_client.py
src/MoCap_*.py src/MoCap_*.py
src/Mocap_*.py src/Mocap_*.py
src/mocap_* src/mocap_*
src/segmented_tether.py
src/segmented_tether_fast.py
msg/Marker.msg msg/Marker.msg
msg/Markers.msg msg/Markers.msg
*.rviz *.rviz

180
README.md
View File

@ -2,8 +2,6 @@
Repo containing oscillation damping controller for tether missions + instructions how to to set up Repo containing oscillation damping controller for tether missions + instructions how to to set up
WARNING: Currently repo is only for Ubuntu 18.04 and ROS Melodic, work is being made to be upgrade to Ubuntu 20.04 and ROS Noetic
Cesar Rodriguez Cesar Rodriguez
cesar.rodriguez@spirirobotics.com cesar.rodriguez@spirirobotics.com
@ -12,69 +10,13 @@ February 2022
Steps to recreate stable PX4 environment + working repo Steps to recreate stable PX4 environment + working repo
### WARNING
Currently repo is only for Ubuntu 18.04 and ROS Melodic, work is being made to be upgraded to Ubuntu 20.04 and ROS Noetic
# Setup # Setup
## 1) Installing ROS Melodic ## 1) Set up oscillation_ctrl
### 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 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 datasets
cd ~/catkin_ws
sudo ./src/mavros/mavros/scripts/install_geographiclib_datasets.sh
### Build source
cd ~/catkin_ws
catkin build
## 2) PX4 Environment Development
Install PX4 firmware and ROS melodic dependencies:
git clone https://github.com/PX4/PX4-Autopilot.git --recursive
git checkout 601c588294973caf105b79a23f7587c6b991bb05
bash ./PX4-Autopilot/Tools/setup/ubuntu.sh
Install [PX4 Firmware](https://docs.px4.io/main/en/dev_setup/dev_env_linux_ubuntu.html#rosgazebo). Then:
#### Download QGroundControl:
cd to desired directory to install QGroundControl. For example, Desktop
cd ~/<desired_directory>
wget https://github.com/mavlink/qgroundcontrol/releases/download/v4.2.0/QGroundControl.AppImage
chmod +x QGroundControl.AppImage
#### Build Gazebo Sim
cd ~/PX4-Autopilot
make px4_sitl gazebo
## 3) Set up oscillation_ctrl
### Install xterm ### Install xterm
sudo apt-get update -y sudo apt-get update -y
sudo apt-get install -y xterm sudo apt-get install -y xterm
@ -83,44 +25,64 @@ cd to desired directory to install QGroundControl. For example, Desktop
cd ~/catkin_ws/src cd ~/catkin_ws/src
git clone https://git.spirirobotics.com/cesar.alejandro/oscillation_ctrl.git git clone https://git.spirirobotics.com/cesar.alejandro/oscillation_ctrl.git
To get the specific version of mavros_msgs for oscillation_ctrl: ### Run bash script and build
bash /.oscillation_ctrl/px4_setup/ubuntu_sim_ros_melodic.sh
This bash script is taken from PX4 Firmware but sets certain versions of dependencies to work with oscillation_ctrl. It also installs common ROS dependencies, ROS Melodic, Gazebo 9, and MAVROS. It should build oscillation_ctrl and add the source path to your .bashrc. If it does not, then run:
cp -R ~/catkin_ws/src/oscillation_ctrl/px4_setup/rosinstall.txt /tmp/mavros.rosinstall cd ~/catkin_ws
wstool merge -t src /tmp/mavros.rosinstall
wstool update -t src -j4
rosdep install --from-paths src --ignore-src -y
catkin build catkin build
source devel/setup.bash
#### Now, we finish building the PX4 package If you do not want to source every time, add 'source ~/catkin_ws/devel/setup.bash' to your .bashrc
## 2) PX4 Environment Development
Install PX4 firmware and dependencies:
git clone https://github.com/PX4/PX4-Autopilot.git --recursive
git checkout 601c588294973caf105b79a23f7587c6b991bb05
bash ./PX4-Autopilot/Tools/setup/ubuntu.sh
There will be prompts on the screen, hit 'u' and 'enter' as they come up.
### Add models, world, and airframe files
To add the necessary Spiri Mu files to PX4, run:
cd ca
bash /.oscillation_ctrl/px4_setup/px4_bash.sh
### Add airframes to cmake targets
Add 'spiri' and 'spiri_with_tether' airframe names in _~/PX4-Autopilot/platforms/posix/cmake/sitl_target.cmake_ under set(models...
__do not add their airframe number! e.i. 4000 or 4001__
### Download QGroundControl:
cd to desired directory to install QGroundControl. For example, Desktop.
cd ~/<example_Desktop>
wget https://github.com/mavlink/qgroundcontrol/releases/download/v4.2.0/QGroundControl.AppImage
chmod +x QGroundControl.AppImage
### Finish Building PX4 package
cd ~/PX4-Autopilot cd ~/PX4-Autopilot
DONT_RUN=1 make px4_sitl_default gazebo DONT_RUN=1 make px4_sitl_default gazebo
If if fails and you see
gzerver: symbol lookup error...
then run:
sudo apt upgrade libignition-math2
If all is good, run:
source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default 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)
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo
roslaunch px4 posix_sitl.launch roslaunch px4 posix_sitl.launch
If this works, we can move on. PX4 should launch with MAVROS
### Add files to 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
cp -R ~/catkin_ws/src/oscillation_ctrl/models/* ~/PX4-Autopilot/Tools/sitl_gazebo/models
cp -R ~/catkin_ws/src/oscillation_ctrl/worlds/* ~/PX4-Autopilot/Tools/sitl_gazebo/worlds
### Add files to ROMFS/px4mu_common
copy (or add) files in _oscillation_ctrl/airframes_ to _PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes_
cp -R ~/catkin_ws/src/oscillation_ctrl/airframes/18.04/* ~/PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes
add model names to _CmakeLists.txt_ in same 'airframe' folder (with number... 4000_spiri and 4001_spiri_with_tether)
add airframe name in _~/PX4-Autopilot/platforms/posix/cmake/sitl_target.cmake_ (no number!)
### Add necessary launch files
this should not be a necessary step and will be changed in the future
copy (or add) files from px4_launch directory to '~/PX4-Autopilot/launch'
cp -R ~/catkin_ws/src/oscillation_ctrl/px4_launch/* ~/PX4-Autopilot/launch
#### Change devel/setup.bash #### Change devel/setup.bash
In catkin_ws (or any working directory) add to devel/setup.bash: In catkin_ws (or any working directory) add to devel/setup.bash:
@ -144,23 +106,13 @@ Finally, source your setup.bash file
- First two elements can be changed to tweak tether parameters - First two elements can be changed to tweak tether parameters
- number_elements: number of segments tether will be composed of - number_elements: number of segments tether will be composed of
- tl: segment length (should be no shorter than 0.3 meters) - tl: segment length (should be no shorter than 0.3 meters)
- __IMPORTANT:__ in order for jinja file to work, the following needs to be added to the _CMakeLists.txt_ in the _~/PX4-Autopilot/Tools/sitl_gazebo_ folder. This creates a custom command to build the tether file whenever a change is done to it.
COMMAND # Oscillation_ctrl Info
${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/scripts/jinja_gen.py models/spiri_with_tether/spiri_with_tether.sdf} ${CMAKE_CURRENT_SOURCE_DIR}
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/scripts/jinja_gen.py models/spiri_with_tether/spiri_with_tether.sdf
VERBATIM
_oscillation_ctrl/px4_setup/CMakeLists.txt has these changes therefore all that is needed is:
cp -R ~/catkin_ws/src/oscillation_ctrl/px4_setup/CMakeLists.txt ~/PX4-Autopilot/Tools/sitl_gazebo/
# oscillation_ctrl info
Info pertaining to oscillation_ctrl repo such as what the ROS nodes do, different ROS parameters, etc. Info pertaining to oscillation_ctrl repo such as what the ROS nodes do, different ROS parameters, etc.
## ROS Nodes ## ROS Nodes
### LinkState.py ### LinkState.py
determines payload load angles and their rates (theta and phi) using Gazebo (needs to be made more robust), as well as determines tether length and keeps track of variables needed in case of a step or square test. Determines payload load angles and their rates (theta and phi) using Gazebo (needs to be made more robust), as well as determines tether length and keeps track of variables needed in case of a step or square test.
__Publishes to__: __Publishes to__:
@ -182,7 +134,7 @@ __Subscribes to__:
/reference/waypoints /reference/waypoints
### ref_signalGen.py ### ref_signalGen.py
takes in desired position (xd) and determines smooth path trajectory. Takes in desired position (xd) and determines smooth path trajectory.
__Publishes to__: __Publishes to__:
@ -198,7 +150,7 @@ __Subscribes to__:
/reference/waypoints /reference/waypoints
### klausen_control.py ### klausen_control.py
determines forces on drone needed based on smooth path and feedback to dampen oscillations. From the forces needed, it publishes attitude commands. Determines forces on drone needed based on smooth path and feedback to dampen oscillations. From the forces needed, it publishes attitude commands.
__Publishes to__: __Publishes to__:
@ -213,9 +165,9 @@ __Subscribes to__:
/mavros/imu/data /mavros/imu/data
### set_ploadmass.py ### set_ploadmass.py
sets the payload mass to be _pload_mass_ value in the _spiri_param.yaml_ Sets the payload mass to be _pload_mass_ value in the _spiri_param.yaml_
### path_follow.cpp ### path_follow.cpp
sets the vehicle in OFFBOARD mode (PX4) and takes off to a set height for 25 seconds before starting to publish attitude and thrust commands. Sets the vehicle in OFFBOARD mode (PX4) and takes off to a set height for 25 seconds before starting to publish attitude and thrust commands.
__Publishes to__: __Publishes to__:
@ -262,3 +214,19 @@ __pload_mass:__ sets the payload mass in simulation without having to change the
## Frequent Issues ## Frequent Issues
Will populate this section with frequently faces issues Will populate this section with frequently faces issues
### No header files error:
In oscillation_ctrl/CMakeLists.txt, change:
add_dependencies(pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
to:
add_dependencies(pathFollow_node oscillation_ctrl_generate_messages_cpp)
### Cannot build px4_sitl
Normally, this is solved by running:
make clean
And redo command you were trying to run. If problem persists, rerun ubuntu_sim_ros_melodic.sh and ubuntu.sh and try again.

54
launch/headless_spiri_mocap.launch
View File

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

54
launch/headless_spiri_with_tether_mocap.launch
View File

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

88
launch/mocap_oscillation_damp.launch
View File

@ -1,88 +0,0 @@
<?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="status">
<rosparam file="$(find oscillation_ctrl)/config/mocapLab_params.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>

64
launch/mocap_sim.launch
View File

@ -1,64 +0,0 @@
<?xml version="1.0"?>
<!--
Launch file to use klausen oscillaton damping ctrl in Gazebo
/-->
<launch>
<group ns="status">
<rosparam file="$(find oscillation_ctrl)/config/mocapGazebo_params.yaml" />
</group>
<arg name="model" default="headless_spiri"/>
<node
pkg="oscillation_ctrl"
type="MoCap_Localization_fake.py"
name="fakeMocap_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"
/>
<!-- 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"
/>
<!-- SETS PLOAD MASS -->
<node
pkg="oscillation_ctrl"
type="set_ploadmass.py"
name="set_ploadmass"
output="screen"
/>
<!-- PX4 LAUNCH -->
<include file="$(find oscillation_ctrl)/launch/$(arg model)_mocap.launch">
</include>
<!-- Tf LAUNCH -->
<node pkg="tf" type="static_transform_publisher" name="map_to_pload_imu" args="0 0 0 0 0 0 map pload_imu 10" />
<node pkg="tf" type="static_transform_publisher" name="map_to_pload" args="0 0 0 0 0 0 map payload 10" />
</launch>

20
px4_setup/protobuf_install.txt
View File

@ -1,20 +0,0 @@
sudo apt-get install libprotobuf-dev=3.0.0-9.1ubuntu1 libprotoc-dev=3.0.0-9.1ubuntu1 protobuf-compiler=3.0.0-9.1ubuntu1
commit: 601c588294973caf105b79a23f7587c6b991bb05
Qground control:
wget https://github.com/mavlink/qgroundcontrol/releases/download/v4.2.0/QGroundControl.AppImage
chmod +x QGroundControl.AppImage
mavros/mavlink:
cp -R ~/catkin_ws/src/oscillation_ctrl/bash_scripts/rosinstall.txt /tmp/mavros.rosinstall
wstool merge -t src /tmp/mavros.rosinstall
wstool update -t src -j4
rosdep install --from-paths src --ignore-src -y
catkin build
steps:
wget https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim_ros_melodic.sh
bash ubuntu_sim_ros_melodic.sh
git clone https://github.com/PX4/PX4-Autopilot.git --recursive
git checkout 601c588294973caf105b79a23f7587c6b991bb05

243
src/MoCap_Localization_fake.py
View File

@ -1,243 +0,0 @@
#!/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()
# load angles
self.load_angles.header.stamp = rospy.Time.now()
# twobody status
self.twobody_status.header.frame_id = "/map"
self.twobody_status.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

221
src/mocap_path_follow.cpp
View File

@ -1,221 +0,0 @@
/**
* @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;
}
}
if (waypoint_cl.call(wpoint_srv)){
// check if waypoints have changed
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;
alt_des = wpoint_srv.response.xd.z;
}
// Check if we want to use oscillation controller
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;
}

104
src/mocap_runTest.py
View File

@ -1,104 +0,0 @@
#! /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.xd1 = Point()
self.xd1.x = 0.0
self.xd1.y = -0.5
self.xd1.z = 1.75
self.xd2 = Point()
self.xd2.x = 0.0
self.xd2.y = 2.0
self.xd2.z = 1.75
# Determine if we want to run test with or without controller
################# CHANGE THIS TO CHANGE TYPE Of TEST ###############################
self.change_mode = True # True = Change to oscillation damping mode after wait time
self.multiple_setpoins = True # True - will send multiple setpoints
#####################################################################################
if self.change_mode: self.loginfo_string = 'Attitude mode in...'
else: self.loginfo_string = 'Staying in Position mode.'
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
waypoint_sent = False
while not run_test:
time_left = self.wait_time - (rospy.get_time() - self.tstart)
if not rospy.get_time() - self.tstart > self.wait_time:
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_test1 = 25.0 - rospy.get_time() + self.t_param
time_until_test2 = 30.0 - rospy.get_time() + self.t_param
if time_until_test1 >= 0.0 and not waypoint_sent:
rospy.loginfo('In %.2f\nSending waypoints: x = %.2f y = %.2f z = %.2f',time_until_test1,self.xd1.x,self.xd1.y,self.xd1.z)
elif not waypoint_sent and time_until_test1 < 0.0:
waypoint_sent = True
self.set_waypoint(self.xd1)
if time_until_test2 >= 0.0 and waypoint_sent:
rospy.loginfo('In %.2f\nSending waypoints: x = %.2f y = %.2f z = %.2f',time_until_test2,self.xd2.x,self.xd2.y,self.xd2.z)
elif waypoint_sent and time_until_test2 < 0.0:
self.set_waypoint(self.xd2)
run_test = True
break
def set_waypoint(self,xd):
""" Set waypoints for oscillation controller """
rospy.wait_for_service('/status/waypoint_tracker')
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