# oscillation_ctrl

Repo containing oscillation damping controller for tether missions + instructions how to to set up

Cesar Rodriguez 

cesar.rodriguez@spirirobotics.com

February 2022

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

## 1) Set up oscillation_ctrl
### Install xterm
	sudo apt-get update -y
	sudo apt-get install -y xterm

### Clone oscillation_ctrl
	cd ~/catkin_ws/src
	git clone https://git.spirirobotics.com/cesar.alejandro/oscillation_ctrl.git

### 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:

	cd ~/catkin_ws
	catkin build
	source devel/setup.bash

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
	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
	export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
	export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo
	roslaunch px4 posix_sitl.launch

PX4 should launch with MAVROS

#### 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

Finally, source your setup.bash file

	cd ~/catkin_ws
	source devel/setup.bash

## Jinja tether file
- _spiri_with_tether.sdf.jinja_ can be altered to create desired tether model
- changes need to be made in px4 directory and will only take effect after running: "make px4_sitl gazebo"
	- can do "DONT_RUN=1 make px4_sitl gazebo" to avoid starting px4 shell and gazebo
- 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)

# oscillation_ctrl info
Info pertaining to oscillation_ctrl repo such as what the ROS nodes do, different ROS parameters, etc.

## ROS Nodes
### 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.

__Publishes to__:

	/status/twoBody_status # no longer used but keeps track of payload and vehicle position
	/status/load_angles    # payload angles (and states) relative to vehicle
	/status/path_follow    # boolean to run trajectory test
__Subscribes to__:

	none

### wpoint_tracker.py 
Sets original waypoints to be (in meters): [x=0,y=0,z=5]. This node listens to topic to keep track of desired waypoints. If any other node wants to change the waypoints, they publish to "reference/waypoints" and wpoint_tracker creates these new waypoints.

__Publishes to__:

	none
__Subscribes to__:

	/reference/waypoints 

### ref_signalGen.py
takes in desired position (xd) and determines smooth path trajectory.

__Publishes to__:

	/reference/path         # smooth path

__Subscribes to__:

	/status/load_angles
	/mavros/local_position/pose
	/mavros/local_position/velocity_body
	/mavros/imu/data
	/mavros/state
	/reference/waypoints

### 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.

__Publishes to__:

	/command/att_target # attitude commands

__Subscribes to__:

	/status/load_angles
	/reference/path
	/mavros/local_position/pose
	/mavros/local_position/velocity_body
	/mavros/imu/data

### set_ploadmass.py
sets the payload mass to be _pload_mass_ value in the _spiri_param.yaml_
### 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.

__Publishes to__:

	mavros/setpoint_position/local # needed to hover @ set height	
	mavros/setpoint_raw/attitude   # attitude and thrust commands 	

__Subscribes to__:

	/command/att_target	
	/command/bodyrate_command	
	/mavros/state

## Launching simulation
To launch a simulation, run the following command:

	roslaunch oscillation_ctrl oscillation_damp.launch

This simulation is set to have a Spiri Mu hover at an alitude of 5 m.
The launch file itself has two usable arguments:

__model:__

	spiri_with_tether           # Spiri Mu with a tethered payload			
	spiri                       # Spiri Mu without tethered paylaod
	headless_spiri_with_tether  # headless mode: launches with no Gazebo GUI. This is the default model
	
__test:__

	none   # default
	step   # step input - default is 5 m
	square # square trajectory
To run the simulation with a tethered payload headless mode and perform a step test:

	roslaunch oscillation_ctrl oscillation_damp.launch model:=headless_spiri_with_tether test:=step

Whenever the oscillation_ctrl is used, the scripts are written such that the vehicle will hover for about 30 seconds in "Position Mode". This is used to take advantage of the takeoff procedure PX4 has, as this controller assumes the vehicle is already in flight when determining the necessary thrust. 

## ROS parameters

#### All these values will be under the '/status/' namespace during simulation
__use_ctrl:__ needs to be set to false to take off. Once Spiri has reached desired altitude, it get be set to  'true' to use oscillation damping controller. This is needed as PX4 has a takeoff procedure which is neglected when using the oscillation damping controller due to attitude setpoints needed a thrust value

__pload_mass:__ sets the payload mass in simulation without having to change the spiri_with_tether jinja file (which needs px4 package to be rebuilt in order to make changes)

## Frequent 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.