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

sudo apt install net-tools
cd /etc/netplan
sudo vim 50-cloud-init.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

mkdir -p formation_ws/src
cd formation_ws
catkin_make
mkdir tools

Install OSQP & OSQP-Eigen

1. Install osqp

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

sudo apt install libeigen3-dev
dpkg -L libeigen3-dev

3. install osqp-eigen

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

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

sudo apt install ros-noetic-robot-upstart

Install Python3

sudo apt install python3

Install pip

sudo apt install python3-pip

Install pymap3d

pip install pymap3d

---

For optitrack

Install vrpn-client-ros (on laptop)

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

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

rosrun robot_upstart uninstall flight

To shut down the launch file running in background

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

vim ~/.bashrc

For Wi-Fi NCRL-TWdrone (GPS)

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)

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

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.

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>
    <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