forked from cesar.alejandro/oscillation_ctrl
184 lines
6.9 KiB
Markdown
184 lines
6.9 KiB
Markdown
# 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 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
|
|
|
|
## 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
|
|
### 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
|
|
- copy (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
|
|
- _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 and
|
|
"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/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/twoBody_status
|
|
/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
|
|
/reference/flatsetpoint # needed to determine thrust
|
|
### Subscribes to:
|
|
/status/twoBody_status
|
|
/reference/path
|
|
/mavros/imu/data
|
|
/mavros/local_position/velocity_body
|
|
- 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
|
|
|
|
|