2. Simulation

We first setup TrajBridge in SITL to familiarize ourselves with what it does within simulated environment. The steps are almost identical to the actual flight setup. This section assumes you are on a Ubuntu 22.04 computer, though it should work on Ubuntu 20.04 with some minor tweaks.

To run the SITL we start by setting up the PX4-Autopilot and Gazebo code:

# Install the uXRCE-DDS (PX4-ROS 2/DDS Bridge)
sudo snap install micro-xrce-dds-agent --edge

# Go to your workspace folder
cd ~/<workspace>/

# Clone our version locked copy of the PX4-Autopilot workspace:
git clone https://github.com/StanfordMSL/PX4-Autopilot.git

# Install the PX4-Autopilot submodules
cd PX4-Autopilot
git submodule update --init --recursive

# Check out the trajbridge branch
git checkout feature/trajbridge

Then we set up TrajBridge:

# Go to your workspace folder
cd ~/<workspace>/

# Clone TrajBridge:
git clone https://github.com/StanfordMSL/TrajBridge.git

# Initialize the submodules
cd TrajBridge
git submodule update --init --recursive

# Build
cd TrajBridge (to get to the actual TrajBridge workspace)
colcon build

# Create and activate a conda environment for your package. We call ours sc-env after the SimpleController.
conda create -n sc-env python=3.10 (use 3.8 if you are on Ubuntu 20.04)
conda activate sc-env

# Install the package that your controller resides in using pip. Using SimpleController as an example:
cd ~/<workspace>/TrajBridge/SimpleController
pip install -e .

And you're all set! To run an example:

# In terminal (1) launch the uXRCE agent, pointing locally
micro-xrce-dds-agent udp4 -p 8888

# In terminal (2) launch the quadcopter in Gazebo:
cd ~/<workspace>/PX4-Autopilot
make px4_sitl gazebo-classic

# In terminal (3) launch the trajbridge node:
cd ~/<workspace>/TrajBridge/Trajbridge
source install/setup.bash
ros2 launch px4_comm trajbridge.launch.py

# Make sure terminal (4) is in the sc-env. When the trajbridge state machine shows ready do:
conda activate sc-env
cd ~/<workspace>/TrajBridge/Trajbridge
source install/setup.bash
cd ~/<workspace>/TrajBridge/SplineController/scripts
python spline2position_node.py --traj search --drone fmu

After executing the terminal (3) command, the drone will arm, takeoff and fly to a starting position. Once there, it is ready to receive setpoint commands, which we send it through the terminal (4) command.

Some notes on options:

1. You can change the trajectory you want to fly changing between the file names (with the '.json') in the SplineController/trajectories folder. You can also tweak/create new trajectories by editing the .json files. The flat outputs are, as their name implies, the flat outputs of the quadcopter [x,y,z,yaw] and their derivatives. Do not over-constrain your keyframes (intermediate frames should have the higher derivatives set to null)
2. You can tweak the trajbridge.launch.py to do the following:
   1. drone_name: Appends a drone name to the topics. If "fmu" is used, it does not append anything.
   2. auto_start: Setting this to True automates the drone arming and taking off. You can set this to False to do so manually (useful in hardware)
   3. auto_land: Setting this to True will cause the drone to land once it's done with the trajectory. Setting it to False will lock it at its current position while setting the yaw to the ready waypoint value (setting the position to ready waypoint might be dangerous). You can cause it to land by closing the trajbridge node or via RC.
   4. wp_ready: The ready waypoint [x,y,z,yaw] that the drone will start its trajectory from.
3. When making the px4 firmware you might need to install a few packages that might not be on your system by default:
   1. pip3 install kconfiglib
   2. pip3 install --user jinja2 jsonschema
4. Sometimes, the gcc version that conda calls is not compatible with ROS2. To fix this, update your gcc by doing conda install -c conda-forge gcc=12.1.0
5. You might have to switch from gazebo-igntion (default for Ubuntu 22.04) to gazebo-classic as the SITL setup in the former is not as mature. To do this, do:
   1. sudo apt remove gz-garden
   2. sudo apt install aptitude
   3. sudo aptitude install gazebo libgazebo11 libgazebo-dev
6. If you can't make in PX4-Autopilot folder due to permission issues (and when you try with sudo, it says something like can't find kconfiglib), you need to change permissions for the PX4-Autopilot folder. Do sudo chown -R <username> PX4-Autopilot/
7. Sometime the lockstep in simulation misbehaves so you'll need to turn it off in the model config file:
   1. gedit /Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/iris/iris.sdf.jinja
   2. Search and set <enable_lockstep>0</enable_lockstep> (default is: <enable_lockstep>1</enable_lockstep>)
8. If you want to launch the gazebo simulation with a drone namespace do PX4_UXRCE_DDS_NS=drone5 make px4_sitl gazebo-classic