# 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

## 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
	
## 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 Gazebo Sim
	cd ~/PX4-Autopilot
	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

## 3) oscillation_ctrl Dependencies
### 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
### CLONE OSCILLATION_CTRL
		cd ~catkin_ws/src
		git clone https://git.spirirobotics.com/cesar.alejandro/oscillation_ctrl.git
### 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
### ROMFS/PX4FMU_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/* 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!)
### LAUNCH FILES
- 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
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
- _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)
- __IMPORTANT:__ in order for jinja file to work, the following needs to be added to the _CMakeLists.txt_ (Ln 288 - may change in future) in the _~/PX4-Autopilot/Tools/sitl_gazebo_ folder:

			COMMAND
				${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
			)
- This should be added in the line below VERBATIM in the add_custom_command function, which should be Ln 288)


## ROS NODES
- __LinkState.py__ determines payload load angles and their rates (theta and phi), as well as determines tether length and keeps track of variables needed in case of step test.
### Publishes to:
		/status/twoBody_status # localization and angles
		/status/load_angles	   # payload angles (and tates) relative to vehicle
		/status/path_follow    # boolean to run trajectory test
### Subscribes to:
		none
- __ref_signalGen.py__ takes in desired position (xd) and determines smooth path trajectory.
### Publishes to:
		/reference/path			# smooth path
		/reference/flatsetpoint    	# needed to determine thrust
### 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/quaternions		# attitude commands
### Subscribes to:
		/status/load_angles
		/reference/path
		/mavros/local_position/pose
		/mavros/local_position/velocity_body
		/mavros/imu/data
- node from _mavros_controllers/geometric_controller_ subscribes to _/reference/flatsetpoint_ to determine thrust commands which are published to _command/bodyrate_command_ by default
- __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/quaternions	
		/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	   # default			
		spiri	   
		headless_spiri_with_tether # headless mode: launches with no Gazebo GUI
		headless_spiri 
####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 klausen_dampen.launch model:=headless_spiri_with_tether test:=step