This ROS package can be used to test the localization of a UAS using a vision system which is capable of tracking markers or targets and doing offboard postion control in a fashion similar to what is shown in this video:
This package runs on an offboard computer running ROS and interfaces to a Pixhawk running the PX4 flight stack. All communications between the offboard computer and the Pixhawk are via mavros on the offboard computer.
If this is your first exposure to the Pixhawk or the PX4 flight stack we recommend you start with the extensive online documentation found here:
http://www.pixhawk.org/choice https://pixhawk.org/dev/start
This package runs in ROS and therefore familiarity with ROS is helpful. ROS as it relates to the PX4 flight stack can be found here:
https://pixhawk.org/dev/ros/start
The major components used in testing this package consist of the following:
This packages makes the following assumptions:
The vis_pose_test package consists of the following ROS nodes.
Simple node that takes the raw inputs from the Pixhawk's RC controller and publishes them on the "joy" topic.
This node is used to proxy mavlink messages between the Pixhawk and the ROS apps. For more information see http://wiki.ros.org/mavros. The version provided with this package is an extention of version 0.14.2. It has an additional plugin that supports the new attitude control topic "setpoint_pt_attitude". This topic is simply an exetension of the existing topic "setpoint_attitude/attitude" with the throttle value included.
The core of this package. Used to perform localization and simple motion control based on target poses received from some vision system. This node listens for target poses on the topic "target_pose" which are sent in a so called "Front", "Left", "Up" (FLU) frame with resepct to the UAS camera frame as described above. It takes these FLU coordinates and transforms them into ENU coordinates (fixed inertial frame) with the target at the origin. In order to do these transformations it uses the vehicle's current pose which it gets by listening on the topic "local_position/local". It publishes the vehicle ENU poses to the topic "vision_pose/pose" which are sent to the FCU. This localization is performed in the file locator.cpp.
In addition to the localization described above this node also performs simple offboard control depending upon the position of the offboard control switch. It does this by listening to the topic "joy". The joy topic contains the various joy stick and switch positions of the same RC controller that is used to control the UAS. Just like the Pixhawk supports MANUAL, ALTCTL, and POSCTL this node also supports those same flight modes as implemented in the controller.cpp file. MANUAL mode takes the joystick positions and translates that into attitude control commands that it publishes on the "setpoint_pt_attitude" topic. The ALTCTL and POSCTL takes joystick positions and translates those into position setpoints that it sends on the "setpoint_position/local" topic.
Note that the vision node used to generate the target poses is not included in this package. Vis_pose_test was tested using aruco. In principle any vision system could be used as long as it publishes target poses on the "target_pose" topic using the FLU frame of reference as described above. Whatever vision system is used should generate target or marker poses in the following fashion.
In general this system uses so called offboard control as described here: https://pixhawk.org/dev/offboard_control.
Special attention should be paid to those sections referring to the use of ROS and mavros.
The initial development and testing of vis_pose_test was done using a Jetson TK1 running Ubuntu 14.04.3. The Jetson was interfaced to the Pixhawk via a FTDI USB serial cable that was plugged into the Telemetry 2 port of the Pixhawk. More specific setup instructions are given below.
In addition vis_pose_test has also been run on other platforms including Ubuntu VM's running on a desktop computer. Such configurations are useful for doing development and testing before porting to the target platform. While not described in this document it is possible to interface these desktop VM's to the Pixhawk just like the compnaion computer. While not tested it should also be possible to run vis_pose_test using SITL and a simulation environment.
QGroundControl (QGC) is an operator interface to the UAS that allows you perform a wide range of operations including loading and configuring the firmware of the Pixhawk. Install it according to the instructions here: http://qgroundcontrol.org/
Clone the GitHub repository: git clone https://github.com/AutoModality/Firmware.git -b am_stable_v1.0.1_build This repository contains firmware for the Pixhawk. This version of PX4 SW is a branch of version 1.0.1 PX4 (stable build) and includes some additional parameters and telemetry streams that can aid in the debugging. At some point these features may be merged into a future release of the PX4 fight stack at which time this repository will no longer be necessary. It is not necessary to build this firmware since pre-built firmwafe versions are already included in the repository, but all the source code is available if you want to make modifications.
Load Firmware/Build/px4fmu-v2_default.build/firmware.px4 into the Pixhawk using QGroundControl (QGC). In order for the new parameters to appear in QGC you must load the firmware using QGC.
Use QGC to configure and set all the appropriate parameters in the Pixhawk including vehicle type etc. The file vis_pose_test/sample/sample.param file can be loaded into QGC and used as a starting point, but you almost certainly need to make modifications to suit your UAS. The sample.param file provided was used to fly indoors using an X8 frame. There will be some additional instrcutions on relevant parameters to tune the localization and position control below.
Use QGC to calibrate the sensors and radios.
Configure the system startup files (e.g. /etc/extras.txt) as described here to suit your situation: https://pixhawk.org/dev/system_startup The system configured for the development of vis_pose_test did not require any edits to /etc/extras.txt
A nunmber of parameters should be set using QGC. As a starting point you can use the sample.param file described above. Use caution when loading this parameter file since if you have already made changes to your parameters as part of the above procedures then you will overwirte those changes by loading sample.param. The following discusses considerations for each of the major parameter sections that can be edited using QGC.
System
The System parameters configure the interface between the Pixhawk and the companion computer. The following parameters were used for testing vis_pose_test. You may need to use different values depending upon how you interfraced your compnaion computer to the Pixhawk
- SYS_AUTOSTART 12001
- SYS_COMPANION 921600
- SYS_RESTART_TYPE 0Radio Switches
Set up the RC radio switches to switch into various flight modes. There are the normal onboard Pixhawk flights modes (MANUAL, ALTCTL, POSCTL) and there are offboard flight modes (MANUAL, ALTCTL, POSCTL) that mirror the same onboard functionality except using offboard setpoint commands. Vis_pose_test was tested using a Spektrum DX8 with the following switch configuration. Consult the sample.param file.
- A three position switch to switch between MANUAL, ALTCTL, and POSCTL. The radio used in the testing of vis_pose_test used the FLAPS switch on the DX8 or this purpose.
- A two position switch used to switch between offboard and ONBOARD-CONTROL. The radio used in the testing of vis_pose_test used the GEAR switch on the DX8 for this purpose. The vis_pose_test node assumes this switch so if you diecide to use a different switch you wil need to change control.cpp appropriately to map a different switch.
- A three position switch used to switch between OFFBOARD-MANUAL, OFFBOARD-ALTCTL, and OFFBOARD-POSCTL. The SW used in testing this package used the AUX 2 switch on the DX8 for this purpose. The vis_pose_test node assumes this switch so if you diecide o use a different switch you wil need to change control.cpp appropriately to map a differetn switch.Position Estimator INAV
Vis_pose_test was tested using the INAV position estimator since that estimator currently supports vision based location inputs. The testing was done indoors with GPS disabled to insure the localization was to a large extent based on the vision based pose information. These parameters should be tuned to best suit your vision system. The following INAV parameters were modified from their default values.
- INAV_W_ACC_BIAS 0.01
- INAV_W_XY_GPS_P 0
- INAV_W_XY_GPS_V 0
- INAV_W_XY_VIS_P 1
- INAV_W_Z_BARO 0.01
- INAV_W_Z_GPS_P 0
- INAV_W_Z_VIS_P 1
- INAV_W_Z_VIS_V 0 (new parameter)Multicopter Positon Control
These parameters control the postion control of the UAS and will dictate how agressively it will respond to positon setpoints. Their values are dependent upon a number of factors including vehicle dynamics and vision system performance. The parameters in smaple.param are intentionally set to give very slow overdamped responsesot setpoints. Once you have your system flying the way you like you can increase the various PID loop gains to change behavior. The following parameters were changed form their default values.
- MPC_MANTHR_MIN 0
- MPC_THR_MAX 1
- MPC_THR_MIN 0
- MPC_XY_P 0.1
- MPC_Z_P 0.5
- MPC_Z_VEL_P 0.15Multicopter Attitude Control
These parameters are completely dependent upon your frame type and UAS performance and should be tuned accordingly. Do not expect the parameters in the sample.param file to work for your paltform and in fact if you have loaded the sample.param file you should as a first step change any of the parameters that are different from their defajult values back to their defaults.
Attitude Estimator EKF
A new parameter (ATT_VIS_EN) was added to this attitude estimator to diable the vision inputs use in the estimator.
This section describes setting up the companion computer.
The companion computer used to test vis_pose_test was interface to the Pixwak using an FTDI USB serial cable connected to the TELEM 2 part of the Pixhawk as described here https://pixhawk.org/dev/companion_link.
These instructons assume you are running Ubuntu on the companion computer. If not then adjust the directions accordingly.
(adapted from http://wiki.ros.org/jade/Installation/Ubuntu)
$ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
$ sudo apt-key adv --keyserver hkp://pool.sks-keyservers.net --recv-key 0xB01FA116
$ sudo apt-get update
$ sudo apt-get install ros-jade-desktop
$ sudo rosdep init
$ rosdep update
$ echo "source /opt/ros/jade/setup.bash" >> ~/.bashrc
$ source ~/.bashrc
$ sudo apt-get install python-rosinstallNote that the version used to test vis_pose_test uses an extension to version 0.14.2 of mavros. In addition the version of mavlink that was tested was Mavlink 2015.8.8. The extension to mavros is simply a new plugin that adds a new topic for attitude control and should be compatible with future version of mavros. The official ROS repositories take time to get upddated with newer releases and you will find that different platfroms get updated at different rates. Therefore to insure that you are getting the correct version of both mavros and mavlink that are compatible with the version of Firmware loaded into the Pixhawk we recommend that you build them from sources according to the directions below.
$ mkdir -p ~/mav_catkin_ws/src
$ cd ~/mav_catkin_ws/src
$ wstool init
$ git clone https://github.com/AutoModality/mavros -b am_master
$ rosinstall_generator mavlink | tee /tmp/rosinstall.yaml
$ wstool merge /tmp/rosinstall.yaml
$ wstool up -j4
$ cd ..
$ catkin build
$ sudo usermod -aG dialout (username used to log into Ubuntu)Note that since it is unlikely that changes will be required to mavros and because it uses a different tool chain to build than the officialy released ROS tool chain we are using the concpet of overlaid packages to seperate the vis_pose_test catkin environment from the mavros environment. This approach is adapted from http://wiki.ros.org/catkin/Tutorials/workspace_overlaying.
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ catkin_init_workspace
$ cd ..
$ source ~/mav_catkin_ws/devel/setup.sh
$ catkin_make
$ echo "source ~/catkin_ws/devel/setup.sh" >> ~/.bashrcIn a new shell:
$ cd ~/catkin_ws/src
$ git clone https://github.com/AutoModality/vis_pose_test
$ cd ..
$ catkin_makeFor those that like to use Eclipse for their development please see the following instructions.
Install Java
$ sudo apt-get install openjdk-7-jreInstall Eclipse
Download Eclipse IDE for C/C++ Developers, Linux, 64 bit from https://eclipse.org/downloads/.
Install (assuming tarball is in ~/Downloads):
$ cd ~
$ tar xzvf Downloads/eclipse-cpp-mars-R-linux-gtk-x86_64.tar.gzConfigure Eclipse ROS Integration (adapted from http://wiki.ros.org/IDEs)
Create the Eclipse project file
$ cd ~/catkin_ws
$ catkin_make --force-cmake -G"Eclipse CDT4 - Unix Makefiles"
$ awk -f $(rospack find mk)/eclipse.awk build/.project > build/.project_with_env && mv build/.project_with_env build/.project
$ cd build
$ cmake ../src -DCMAKE_BUILD_TYPE=DebugStart Eclipse (the source command can be skipped if you already have it in your ~/.bashrc)
$ source ~/catkin_ws/devel/setup.sh
$ cd ~/eclipse
$ ./eclipseEnter "/home/ubuntu/catkin_ws" for the "Workspace" and click "OK"
Select "File->Import...->General->Existing Projects into Workspace” and click "Next"
Right click on "build" and select "properties"
Select “c++ Include Paths and Symbols”
Click on the “Add External Include Path...” button
Repeat for:
Click "OK"
Editing Files
Edit files by expanding the "build->[Source directory]" in the "Project Explorer" and navigating to the file.
Building
Select "Project->Build All" or press Ctrl-B.
Cleaning
Right click on the "build" project and select "Clean Project".
The launch files for the various ROS nodes that comprise this application can be found in "vis_pose_test/launch". In there you will also find script files for launching the various nodes as described below. You may have to make the following edits to the launch files:
To run everything follow these steps.
Open an ssh console to the companion computer and run mavros:
$ cd catkin_ws/src/vis_pose_test/launch $./lmv
At this point QGC should connect to the Pixhawk and you should hear the ennuciation from QGC that it is connected. If mavros launches correctly and you get the QGC enunciation then a lot of things are working correctly. Don't proceed until this works. Note that the mavros.lauch file launches both the mavros node and the rc_controller node. If for some reason mavros fails to launch check the following:
Open an ssh console to the companion computer and run vis_pose_test:
$ cd catkin_ws/src/vis_pose_test/launch $./lvp
At this point vis_pose_test shgould indicate that it is running and assuming it is getting target poses from the vision software it should be forwarding vision based locations to the Pixhawk. If for some reason you are getting watchdog timeout messages every few seconds this indicates that vis_pose_test is not getting any messages from the rc controller. Make sure it is on and connected to the Pixhawk. If you desire more output to see what is happening in vis_pose_test use ROS to turn on the ROS_DEBUG messages and
Before attempting any flights you should do extensive bench testing to insure that the localization and control are working properly. The steps below will help insure things are operating properly
The first step to validating that the locaization is working properly is to use the QGC Analyze view to plot the following:
The VISION_POSITION_ESTIMATE represents the UAS poses wrt to the target that vis_pose_test is sending to the Pixhawk and the LOCAL_POSIITION_NED is where the Pixhawk thinks it is based upon a number of inputs including the vision estimate. If everything is working properly the LOCAL_POSIITION_NED and the VISION_POSITION_ESTIMATE will correspond closely. If for some reason you can not find the VISION_POSITION_ESTIMATE in the list of telemetry streams displayed by QGC then this indicates that the Pixhawk is not receiving any VISION_POSITION_ESTIMATE updates from the companion computer. Open an ssh console on the companion comuter and use the rostopic tool to trace the message flows through the system. Make sure the proper messages are flowing from the vision system to vis_pose_test and likewise that vis_pose_test is publishing vison estimates to mavros.
Assuming you are getting plots in QGC that indicate that vision estimates are coming in and that the Pixhawk is using them to estimate it's position then try flying the UAS by hand and/or moving the target and observe how the locations being plotted in QGC change. Remember that the Pixhawk is in NED so in order to work out if the right cordinates are changing as exepcted when moving the UAS or target you must know which way the UAS is oriented. Also bear in mind that the poses from vis_pose_test are with respect to the target meaning that the target is at the origin. This can be a little couterintuitive. For example if the UAS is oriented north with the target directly in front of it then the x coordinate being plotted in QGC should be negative, i.e. the UAS is south of its local origin. Furthermore if the target moves closer to the UAS then the x coordinate will "increase", i.e. corresponding to moving in a northerly direction. Also assuming all motion is in a north south direction then x coordinate should change and y should remain relatively unchanged. Assuming the same configuration the y cooridinate should be around 0 and if you move the UAS left (target right) then the y coodinate should decrease becasue the UAS it is moving in a westerly direction from the origin. Try this same sort of motion through all the different quadrants to make sure they are changing as expected.
The next step is to validate the magnitude of the changes as the UAS moves. One way to do this is to get a long board that you can set the UAS (or target) on and mark out measurements along the board. Orient the board along one of the compass axis and moved the UAS (or target) along the board in very well defined increments and see if the corresponding axis changes as expected in QGC by that amount. When orienting the board make sure that you orient it as per what the UAS thinks is North/South/East/West by using QGC and not what some third party compass might say. It is not as important to orient things along true north as it is to orient then in a fashion that is consistent with the UAS. Do this same test along a number of different orientations North/South or East/West orientations.
As a final test put the UAS on one end of the board and the target on the other so that they are facing each other. Fix the end of the board with the target at some point (e.g. a chair that swivels) and rotate the end with the UAS around the fixed target in a circle. If you log the LOCAL_POSIITION_NED data either in QGC or in the Pixhawk logs on the SD card then you should be able to plot x against y and see a perfect circle. There are utilites for exporting Pixhawk log data to Matlab that makes this process relatively straightforward. If you repest the test with the UAS at the center and the target being rotated at the other end of the board you should get the same result.
The control loop in controller.cpp is driven by messages received on the joy topic from the rc_joystick node. Every time a message is received on the joy topic then a setpoint message is sent to the Pixhawk. If you want to change the frequency of setpoint messages then you must change the frequency of joy messages. This also means that if joy messages are not being received by vis_pose_test then no setpoint message will be sent to the Pixhawk. This may happen if the rc controller is not powered on or if the rc_joystick ROS node is not operating properly.
There is a watchdog timer in vis_pose_test that prints a warning to the console if setpoint messages are not been sent by vis_pose_test in a timely fashion. If you do not see watchdog timeout messages being printed on the console then setpoint messages are being published by vis_pose_test. The Pixhawk will not go into or stay in OFFBOARD mode unless it receives setpioint messages on a regular basis. To confirm that the Pixhawk is in fact receiving the setpoint message you need only move the offboard switch on the rc controller to the OFFBOARD position. If the Pixhawk can successfully go into OFFBOARD mode then the QGC will indicate this both with an enunciation and the filght status changing to OFFBOARD. Before arming the UAS and trying to fly, perform the following steps to validate that offboard control is working as expected.
The vis_pose_test node supports the following OFFBOARD flight modes which in general work the same way as the corresponding ONBOARD filight modes.
Moving the offboard flight mode switch to different positions selects between these different modes. When vis_pose_test switches to a different flight mode then it will print on the console which flight mode it is switching to.
As noted they operate the same as the ONBOARD flight modes so if you are familiar with those then you should be able to operate the UAS the same way in offboard mode. One difference is that because the UAS localization is done using the target or marker moving the target or marker while the UAS is in flight should casue the UAS to also move to maintain the same setpoint with respect to the target.
To test fly do the following steps.
FAIL SAFES
There are fail safes built into the system such that if vis_pose_test detects the loss of marker or target acquisition by the vision system then it stops sending setpoints to the Pixhawk. Vis_pose_test detects the loss of target acquisition when it receives an empty list from the vision system. There are currently no timers in vis_pose_test that detect when the vision_pose/pose topic has not been updated in a timely fashion although these can be easily added if desired. When vis_pose_test does go into failsafe mode the setpoints will cease to be sent to the Pixhawk and the watchdog timeouts will be dispalyed on the screen. This will cause the Pixhawk to drop out of OFFBOARD mode and you should get the enunciation "REJECT OFFBOARD". Note that when the Pixhawk drops out of OFFBOARD mode it will try to drop into whatever flight mode indicated by the onboard flight mode switch so it is probably desireable to have it in the MANUAL position. Once the vis_pose_test goes into failsafe mode you can not resume normal operation until you set the offboard switch to ONBOARD AND you set the offbaord flight mode switch to MANUAL.