Pixhawk4 Vision Control

This repository explains how to integrate a Pixhawk4-controlled vehicle with OptiTrack Motive system.

Architecture

Software Architecture

Hardware Architecture

The Setup

Requirements

• Windows PC running MATLAB/Simulink 2021b and OptiTrack Motive
• Linux PC with Ubuntu 16.04 and ROS Kinetic
• Holybro S500 drone with Pixhawk4 and radio telemetry
• USB WiFi adapter (we used TP-Link AC1900

Note: The USB WiFi adapter is not needed if the Linux and Windows PC exist in the same network (i.e. connected to an ethernet switch)

Hint: You may run a Linux virtual machine within the Windows PC and perform the steps below without needing another Linux PC. Make sure that network is well configured, and USB devices are enabled on the virtual machine.

Installation

1. Install mavros and mavros_extras ROS(1) packages¹

  sudo apt-get install ros-kinetic-mavros ros-kinetic-mavros-extras

2. Install vrpn_ros_client ROS(1) packages

  sudo apt-get install ros-kinetic-vrpn-client-ros

3. Clone this repo in your catkin_ws directory

  cd ~/catkin_ws/src
  git clone https://github.com/walmaawali/px4_vision_control.git

4. Build the workspace²

  cd ~/catkin_ws
  catkin_make

5. Source the setups to bashrc so that the px4_vision_control package is searchable³

  sudo echo 'source ~/catkin_ws/devel/setup.bash' >> bashrc

It's good idea to close and reopen all termianls when running step 5

Testing the integration

1. Power on the drone
2. Plug the USB telemetry radio. On the termial, enable access to the device

  sudo chmod 666 /dev/ttyUSB0

Note: the USB telemetry radio may be on a different directoty (i.e. /dev/ttyUSB1 or /dev/ttyACM0). Run ls /dev to see all serial devices. Try unplug and replug the USB telemetry radio and see which file gets created.

3. Launch mavros node

  roslaunch mavros px4.launch

4. Open another terminal, and issue arming command

  rosrun mavros mavsafety arm

5. To disarm, use the command

  rosrun mavros mavsafety disarm

Note: issuing the arming command may be rejected. If the drone produces a "rejection" sound, the integration still is successful. The sound means that not all arming conditions are satisfied. You may need to stream motion capture position (more into that later).

Setup Drone for External Position Estimate

1. Download and open QGroundControl.
2. Install fresh firmware and perform calibaration of the drone. See this guide
3. In QGroundControl, go to Vehicle Setup > Parameters
4. Change the following parameters to allow position estimate with external motion capture system:

Parameter	Setting
EKF2_AID_MASK	Set vision position fusion, vision velocity fusion, vision yaw fusion and external vision rotation
EKF2_HGT_MODE	Set to Vision to use the vision a primary source for altitude estimation

Reboot the flight controller in order for parameter changes to take effect.

More info about the setup can found in this reference

Setup OptiTrack Motive

1. Perform setup of the motion capture system. See this guide
2. Perform calibration of the system. See this guide
3. Place (at least three) markers on the drone.

4. Place the drone in the arena. You should see the markers in Motive.
5. Select the markers of the drone (at least three) and right-click in Motive and select Rigid Body -> Create From Selected Markers

6. Open the asset pane click on View -> Asset Pane.

7. The rigid body will appear in Assets in the project pane. Rename the drone to drone1.

Hint: If another name is desired, use a name without a space (for example, use robot_1 or robot1 instead of robot 1).⁴

Keep a note on the name of the drone, as it will be used in ROS.

8. Open the data streaming pane click on View -> Data Streaming Pane.

9. Enable VRPN broadcasting and Frame broadcasting according to the settings below (you need to show advanced settings to see all the fields). Make sure VRPN broadcating port is 3883.

10. Start live streaming by clicking the red dot in OptiTrack Motive

Testing the integration

Note: The following steps are illusterated for WiFi connection between the Linux PC and Windows PC. You may use ehternet connection, ehternet switch, or virtua machine as mentioned earlier.

1. Place the drone in the arean.
2. Connect the USB WiFi adapter to Windows PC, and enable mobile hotspot from the network settings.
3. Connect the Linux PC to the mobile hotspot. Keep a note on the IP addresses of the Windows PC and Linux PC by running this command in Linux terminal

  ifconfig

4. After perfoming Installation steps, run the following command on Linux terminal (change the server IP address 192.168.137.1 to the one obtained from step 3) ⁵

  roslaunch vrpn_client_ros sample.launch server:=192.168.137.1

5. To check that motion data is received by ROS, open another termianl and run

  #running this command should show a new topic /vrpn_client_node/robot1/pose
  rostopic list 

  # you shoud see a stream of data after running this command
  rostopic echo /vrpn_client_node/robot1/pose

Here we have named our rigid body robot1. The name may be different according to the name you have chosen while creating the rigid body.

Note: streaming will stop if you take out the drone from the arena.

6. (Optional) To visuaize data in ROS, run the following commands, each one in a seperate terminal

  #convert from OptiTrack coordinate system to ROS coordinate system
  rosrun tf static_transform_publisher 0 0 0 1.57 0 1.57 map world 10

  # visuaizatin tool in ROS
  rviz

Next, add a Pose and tf objects in RViZ, according to the image below. The object in RViZ should move as you move or rotate the drone in the arena.

More info can be found in this reference. See VRPN section.

Put All Together

For safety purposes, you should do this test by removing drone propellers

1. Power on the drone and place it in the arena
2. Connect the USB radio telemetry to Linux PC. Open a terminal and run

  sudo chmod 666 /dev/ttyUSB0

3. Make sure OptiTrack Motive is running. Run the following command in the terminal

  roslaunch px4_vision_control setup.launch

It's good idea to check Motive data streaming and arming commands as we did earlier

4. (Optional) Check that motion capture data is being forwarded to the drone by running

  # you should see a stream of data when running this command
  rostopic echo /mavros/pose_vision/pose

5. Launch MATLAB/Simulink in Windows PC. Download and open this Simulink Model

6. Open the Apps tab, select Robotic Operating System (ROS), and make the following configurations (here the IP address of the Linux PC is 192.168.137.5. Change it to yours)

It's good idea to click "Test connection" to ensure MATLAB and ROS are well connected

7. Double click on the "ROS Publish" and "ROS Message" blocks, and make sure the following configs are applied

Each input X, Y, Z, and W (yaw) represent a joystick control input (PWM values) to the drone. In this mode, the Z (altitude) is set to a sine wave, which will make the drone fly off the ground and land repeatedly.

The range of effective PWM values was found to be 1100-1800 (experimental).

If the above Simulink ran sccessfully, you can design a controller to output RC joystick values.

8. Arm the drone by running this command in Linux terminal. Then run Simulink model.

  rosrun mavros mavsafety arm

To disarm, run rosrun mavros mavsafety disarm You can only disarm when joystick values are neutral (X=1500, Y=1500, Z=1100, W=1500)

9. (Optional) You can find motion capture data in Simulink by subscribing to vrpn_client_ros/drone1/pose topic

References

OptiTrack Motive

VRPN ROS Package

MAVROS

How to setup motion capture with ROS

How to setup PX4 for motion capture

Authors and Contributers

• Dr. Jawhar Ghommam
• Ibrahim Al Jahwari
• Waleed Al Maawali
• Madona Ibrahim
• Majid Al Mujaini

Footnotes

1. You can also install from the source using this guide ↩

2. You could also build the workspace using catkin build if you installed python-catkin-tools ↩

3. Or alternatively, you could run source ~/catkin_ws/devel/setup.bash when opening every new terminal in Ubuntu. ↩

4. Currently, the name drone1 is hard-coded. If you used another name, change drone1 to the drone name in files scripts/motive_topic_relay.py and launch/setup.launch of this package. ↩

5. Currently, the IP address 192.168.137.1 of Windows PC is hard-coded. If yours is different, make sure to change this in the file launch/setup.launch of this package. ↩