# 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
> "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
```
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
```
* **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
Sign in with Wallet
Connect another wallet