---
# System prepended metadata

title: NCRL_Formation with optitrack & GPS
tags: [NCRL-formation]

---

# NCRL_Formation with optitrack & GPS

###### tags: `NCRL-formation`

## Prerequisite (For all laptop && R-PI)

### Install ubuntu 20.04
- Use raspberry-pi imager 
    - https://www.raspberrypi.com/software/
### Network setting
```cmd=
sudo apt install net-tools
cd /etc/netplan
sudo vim 50-cloud-init.yaml
```
```yaml=
network:
        ethernets:
                eth0:
                        dhcp4: true
                        optional: true
        wifis:
                wlan1:
                        dhcp4: true
                        dhcp6: true
                        access-points:
                                "{your wifi name}":
                                        password: "{your wifi password}"
               #wlan0:
               #         dhcp4: false
               #         dhcp6: false
               #         addresses: [192.168.4.160/24]
               #         gateway4: 192.168.4.1
               #         access-points:
               #                 "AUVLAB-exp":
               #                         password: "auvlab12345678"
        version: 2
```
Note: If you don't have Wi-Fi dongle, use **wlan0** instead.

### Install ROS noetic && make your ROS workspace
http://wiki.ros.org/noetic/Installation/Ubuntu

Note: In *1.4 Installation*, choose **ROS-Base**(No GUI tools) version to install
```cmd=
mkdir -p formation_ws/src
cd formation_ws
catkin_make
mkdir tools
```
### Install OSQP & OSQP-Eigen
1. Install osqp
```cmd=
cd ~/formation_ws/tools
git clone --recursive https://github.com/oxfordcontrol/osqp
cd osqp
mkdir build && cd build
cmake .. -DBUILD_SHARED_LIBS=ON
make -j8
sudo make install
```
2. install eigen3
```cmd=
sudo apt install libeigen3-dev
dpkg -L libeigen3-dev
```
3. install osqp-eigen
```cmd=
cd ~/formation_ws/tools
git clone https://github.com/robotology/osqp-eigen.git
cd osqp-eigen
mkdir build && cd build
cmake ../
make -j8
sudo make install
```

### Install MAVROS
```cmd=
cd ~/formation_ws/tools
sudo apt-get install ros-noetic-mavros ros-noetic-mavros-extras
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
sudo bash ./install_geographiclib_datasets.sh   
```

### Install robot_upstart 
```cmd=
sudo apt install ros-noetic-robot-upstart
```

### Install Python3
```cmd=
sudo apt install python3
```

### Install pip
```cmd=
sudo apt install python3-pip
```

### Install pymap3d
```cmd=
pip install pymap3d
```
---

## For optitrack

### Install vrpn-client-ros (on laptop)
```cmd=
git clone https://github.com/ros-drivers/vrpn_client_ros.git
sudo apt-get install ros-noetic-vrpn-client-ros
sudo apt-get install ros-noetic-roslint
vim vrpn_client_ros/launch/sample.launch
```
change server IP

![](https://i.imgur.com/GBSiKSS.png)
> "192.168.2.195" -> "192.168.4.182"

### Pixhawk parameters setting
- SER_TEL1_BAUD: 230400
- MAV_0_RATE: 11520
- EKF2_AID_MASK: 24
    - vision position fusion
    - vision yaw fusion
- EKF2_HGT_MODE: vision
- COM_ARM_WO_GPS: Enabled
- MAV_X_MODE: onboard
- SYS_HAS_GPS: Disabled
- UAVCAN_SUB_GPS: Disabled
- CBRK_IO_SAFETY: 22027 //don’t need safety switch
- MAV_SYS_ID: X (define the number of the UAV)
---

## For GPS

### Pixhawk parameters setting
- SER_TEL1_BAUD: 230400
- MAV_0_RATE: 11520
- EKF2_AID_MASK: 1 (or 33)
    - GPS
    - (multi-rotor drag fusion)
- EKF2_HGT_MODE: Barometric pressure
- COM_ARM_WO_GPS: Enabled
- MAV_X_MODE: onboard
- SYS_HAS_GPS: Enabled
- UAVCAN_SUB_GPS: Enabled
- CBRK_IO_SAFETY: 22027 //don’t need safety switch
- MAV_SYS_ID: X (define the number of the UAV)

---

## NCRL-Formation user guide

### Robot_upstart settings
To add robot_upstart progress
```cmd=
rosrun robot_upstart install --master "http://192.168.50.86:11311" --setup /home/ubuntu/formation_ws/devel/setup.bash flight_control/launch/formation_cbf.launch --logdir ~/robot_upstart_logs/formation_cbf_logs
sudo systemctl daemon-reload && sudo systemctl start flight
```
To cancel robot_upstart progress
```cmd=
rosrun robot_upstart uninstall flight
```
To shut down the launch file running in background
```cmd=
cat ~/robot_upstart_logs/formation_cbf_logs/flight.pid #get progress id of launch file
sudo kill -9 #id_u_get
```
指令说明：
参数
**--master**：ROS_MASTER_URI
**--setup**：The ros environment u want to add, we choose setup.bash, to which flight_control package belongs
**--logdir**：Logs directory, it will spawn itself
**flight_control/launch/formation_cbf.launch**: The path to your launch file, it is nessessory that it should be started up with your package name

### Network settings
Use *ifconfig* to check your ip4 address, then change your ROS_IP in ~/.bashrc
```cmd=
vim ~/.bashrc
```
![](https://i.imgur.com/7iyPsvx.png)
For Wi-Fi NCRL-TWdrone (GPS)
```bash=
export ROS_Master_URI=http://192.168.50.xx #Master's IP4 address (probably your laptop IP4 address)
export ROS_IP=192.168.50.xx (self Ip-address)
```
For Wi-Fi NCRL-GPS (Optitrack)
```bash=
export ROS_Master_URI=http://192.168.4.xx #Master's IP4 address (probably your laptop IP4 address)
export ROS_IP=192.168.4.xx (self Ip-address)
```


### Modify px4.launch
```cmd=
roscd mavros
sudo vim launch/px4.launch
```
![](https://i.imgur.com/trJMF75.png)
* **fcu_url**: *ttyXXX* depends on the UART port you use, and 230400 is the baud rate which should match the parameter of pixhawk setting: SER_TEL1_BAUD
* **gcs_url**: The address is set to the laptop IP4 address where you run *QGroundControl*
* **tgt_system**: It should match the parameter of pixhawk setting: MAV_SYS_ID



### Download code from github
Since this is a package, you should download it to /src under your workspace.
```cmd=
cd ~/formation_ws/src
git clone https://github.com/AMNshaw/ncrl_formation.git
cd ncrl_formation
git checkout master #There are different branches for different usage
cd ../.. && catkin_make
```
- Branch:
    - **master** is for QAV250 with position feedback obtained from optitrack
    - **TWdrone** is for H6F920 with position feedback obtained from optitrack
    - **TWdrone-GPS-leader** is for leader with positon feedback obtained from GPS
    - **TWdrone-GPS-folloer** is for follower with positon feedback obtained from GPS

### Modify formation.launch
```launch=
<launch>
    <group ns="MAV2">
        <param name="UAV_ID" value="2"/>
        <param name="obs_safe_D" value="3"/>
        <param name="obs_gamma" value="0.8"/>
        <param name="MAV_safe_D" value="3"/>
        <param name="MAV_gamma" value="0.8"/>
        <param name="delay_step" value="0"/>
        <rosparam param="laplacian">
                        [1,0,0,0,0,
                         1,1,0,0,0,
                         1,1,1,0,0,
                         1,1,0,1,0,
                         1,1,0,0,1]</rosparam>

        <param name="pub_pose_topic" value="/MAV2/mavros/global_position/ENU/pose"/>
        <param name="gps_global_topic" value="/MAV2/mavros/local_position/pose"/>
        
        <include file="$(find mavros)/launch/px4.launch">
        </include>
        <node name="gps_pub" pkg="flight_control" type="gps_pub"></node>
        <node name="formation" pkg="flight_control" type="formation"></node>
        <node name="velocity_cbf_generator" pkg="flight_control" type="velocity_cbf_generator" output="screen">
            <rosparam subst_value="true">
                use_input: "velocity"  #velocity,position
           </rosparam>
       </node>
    </group>
</launch>

```
Change the number of UAV_ID and number of MAV according to the MAV_SYS_ID in pixhawk setting

### Run process

For each R-PI, run following command:
```=
roslaunch flight_control formation_cbf.launch
```
For ground station, run following command:
```=
rosrun flight_control leader_pose
```
### Important code
* ncrl_formation/src/gps_pub.cpp
* ncrl_formation/src/lla2enu.py
* ncrl_formation/node/formation_node.cpp
* ncrl_formation/node/velocity_cbf_generator_node.cpp

