This repository explains how to integrate a Pixhawk4-controlled vehicle with OptiTrack Motive system.
- 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.
- Install mavros and mavros_extras ROS(1) packages1
sudo apt-get install ros-kinetic-mavros ros-kinetic-mavros-extras
- Install vrpn_ros_client ROS(1) packages
sudo apt-get install ros-kinetic-vrpn-client-ros
- Clone this repo in your
catkin_ws
directory
cd ~/catkin_ws/src
git clone https://github.com/walmaawali/px4_vision_control.git
- Build the workspace2
cd ~/catkin_ws
catkin_make
- Source the setups to bashrc so that the
px4_vision_control
package is searchable3
sudo echo 'source ~/catkin_ws/devel/setup.bash' >> bashrc
It's good idea to close and reopen all termianls when running step 5
- Power on the drone
- 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
). Runls /dev
to see all serial devices. Try unplug and replug the USB telemetry radio and see which file gets created.
- Launch mavros node
roslaunch mavros px4.launch
- Open another terminal, and issue arming command
rosrun mavros mavsafety arm
- 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).
- Download and open QGroundControl.
- Install fresh firmware and perform calibaration of the drone. See this guide
- In QGroundControl, go to Vehicle Setup > Parameters
- 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
- Perform setup of the motion capture system. See this guide
- Perform calibration of the system. See this guide
- Place (at least three) markers on the drone.
- Place the drone in the arena. You should see the markers in Motive.
- Select the markers of the drone (at least three) and right-click in Motive and select
Rigid Body -> Create From Selected Markers
- Open the asset pane click on
View -> Asset Pane
.
- The rigid body will appear in
Assets
in the project pane. Rename the drone todrone1
.
Hint: If another name is desired, use a name without a space (for example, use
robot_1
orrobot1
instead ofrobot 1
).4
Keep a note on the name of the drone, as it will be used in ROS.
- Open the data streaming pane click on
View -> Data Streaming Pane
.
- 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.
- Start live streaming by clicking the red dot in OptiTrack Motive
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.
- Place the drone in the arean.
- Connect the USB WiFi adapter to Windows PC, and enable mobile hotspot from the network settings.
- 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
- 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) 5
roslaunch vrpn_client_ros sample.launch server:=192.168.137.1
- 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.
- (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.
For safety purposes, you should do this test by removing drone propellers
- Power on the drone and place it in the arena
- Connect the USB radio telemetry to Linux PC. Open a terminal and run
sudo chmod 666 /dev/ttyUSB0
- 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
- (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
- Launch MATLAB/Simulink in Windows PC. Download and open this Simulink Model
- 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
- 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.
- 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)
- (Optional) You can find motion capture data in Simulink by subscribing to
vrpn_client_ros/drone1/pose
topic
How to setup motion capture with ROS
How to setup PX4 for motion capture
Footnotes
-
You could also build the workspace using
catkin build
if you installed python-catkin-tools ↩ -
Or alternatively, you could run
source ~/catkin_ws/devel/setup.bash
when opening every new terminal in Ubuntu. ↩ -
Currently, the name
drone1
is hard-coded. If you used another name, changedrone1
to the drone name in files scripts/motive_topic_relay.py and launch/setup.launch of this package. ↩ -
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. ↩