This package is a fork of the popular tum_ardrone package. Currently, the changes are:
This package contains the implementation corresponding to the following publications:
You can find a video on youtube. This Package builds on the well known monocular SLAM framework PTAM presented by Klein & Murray in their paper at ISMAR07. Please study the original PTAM website and the corresponding paper for more information on this part of the software. Also, be aware of the license that comes with it.
The code works for both the AR.Drone 1.0 and 2.0, the default-parameters however are optimized for the AR.Drone 2.0 by now.
cd catkin_ws/src
git clone https://github.com/thinclab/uga_tum_ardrone.git -b indigo-devel
cd ..
rosdep install uga_tum_ardrone
catkin_make
roslaunch uga_tum_ardrone ardrone_driver.launch
roslaunch uga_tum_ardrone uga_tum_ardrone.launch
On the GUI, under Drone Communication Status, you should see:
Estimates the drone's position based on sent navdata, sent control commands and PTAM.
IMPORTANT: requires messages to be sent on both /ardrone/navdata (>100Hz) and /ardrone/image_raw (>10Hz), i.e. a connected drone with running ardrone_autonomy node, or a .bag replay of at least those two channels. ardrone_autonomy should be started with:
rosrun ardrone_autonomy ardrone_driver _navdata_demo:=0 _loop_rate:=500
None
~publishFreq: frequency, at which the drone's estimated position is calculated & published. Default: 30Hz
~calibFile: camera calibration file. If not set, the defaults are used (camcalib/ardroneX_default.txt).
UseControlGains: whether to use control gains for EKF prediction.
UsePTAM: whether to use PTAM pose estimates as EKF update
UseNavdata: whether to use Navdata information for EKF update
UsePTAM and UseNavdata are set to false, the EKF is never updated and acts as a pure simulator, prediciting the pose based on the control commands received (on /cmd_vel). Nice for experimenting.
PTAMMapLock: lock PTAM map (no more KF)
PTAMSyncLock: lock PTAM map sync (fix scale and pose offsets etc.)
PTAMMaxKF: maximum amount of KF PTAM takes.
PTAMMinKFDist: min. distance between two KF (in meters)
PTAMMinKFWiggleDist: min. distance between two KF (relative to mean scene depth).
PTAMMinKFTimeDiff: min time between two KF.
PTAM takes a new KF if (PTAMMinKFTimeDiff AND (PTAMMinKFDist OR PTAMMinKFWiggleDist)), and tracking is good etc.
RescaleFixOrigin: If the scale of the Map is reestimated, only one point in the mapping PTAM <-> World remains fixed. If RescaleFixOrigin == false, this is the current pos. of the drone (to avoid sudden, large "jumps"). this however makes the map "drift". If RescaleFixOrigin == true, by default this is the initialization point where the second KF has been taken (drone pos. may jump suddenly, but map remains fixed.). The fixpoint may be set by the command "lockScaleFP".
MinTolerance: min. scale to accept, prevents initialization failures that could cause the drone to crash (physically)
MaxTolerance: max. scale to accept, prevents initialization failures that could cause the drone to crash (physically)
c1 ... c8: prediction model parameters of the EKF. see "Camera-Based Navigation of a Low-Cost Quadrocopter"
zDriftThreshold: ignore changes in Z below this threshold when landed, prevents drifting when on the ground and tracking is lost
yawDriftThreshold: ingore changes in Yaw below this threshold when landed, prevents drifting when on the ground and tracking is lost
TODO
TODO
To properly estimate PTAM's scale, it is best to fly up and down a little bit (e.g. 1m up and 1m down) immediately after initialization. There are two windows, one shows the video and PTAM's map points, the other one the map. To issue key commands, focus the respective window and hit a key. This generates a command on /uga_tum_ardrone/com, which in turn is parsed and does something.
Key | /tum_adrone/com message | Action |
---|---|---|
r | "p reset" | resets PTAM |
u | "p toggleUI" | changes view |
space | "p space" | takes first / second Keyframe for PTAM's initialization |
k | "p keyframe" | forces PTAM to take a keyframe |
l | "toggleLog" | starts / stops extensive logging of all kinds of values to a file |
m | "p toggleLockMap" | locks map, equivalent to parameter PTAMMapLock |
n | "p toggleLockSync" | locks sync, equivalent to parameter PTAMSyncLock |
Clicking on the video window will generate waypoints, which are sent to drone_autopilot (if running):
Key | /tum_adrone/com message | Action |
---|---|---|
r | "f reset" | resets EKF and PTAM |
u | "m toggleUI" | changes view |
v | "m resetView" | resets viewpoint of viewer |
l | "toggleLog" | starts / stops extensive logging of all kinds of values to a file |
v | "m clearTrail" | clears green drone-trail |
Critically damped spring based controller for the drone (see: Wikipedia: Damping ). Also includes basic way-point-following and automatic initialization. Requires drone_stateestimation to be running. The target is set via the /uga_tum_ardrone/com topic.
Two dimensions (Yaw and Z) can be controlled directly by the drone's motors. The other two (X and Y) are controlled by the drone "leaning" in that direction, which complicates control slightly. The damped springs provide a target force that the drone tries to generate with its motors and leaning, while still maintaining it's altitude.
None
~minPublishFreq: usually, a control command is sent for each pose estimate received from drone_stateestimation. However, if no pose estimate is received for more than minPublishFreq milliseconds, a HOVER command is sent, causing the drone to hover if for example drone_stateestimation is shut down suddenly. Default: 110.
K_direct, K_rp: spring strength of the directly controlled dimensions (yaw and Z) and the leaning dimensions (X and Y) respectively
droneMassInKilos: needed for proper damping
max_yaw: can be used to limit the speed of yaw'ing (the drone can spin quite fast!)
xy_damping_factor: due to the use of leaning the drone can only roughly approximate a damped spring, tune this parameter to account for things like air resistance and wind
max_rp_radians: this should match the corresponding parameter in ardrone_autonomy
max_gaz_rise: limit the maximum positive Z speed (m/s)
max_gaz_drop: limit the maximum negative Z speed (m/s)
max_rp: limit the maximum leaning command sent (x and y)
rise_fac: rise commands are larger than respective drop commands by this factor. This is due to the drone sometimes dropping unpredictably fast for large drop commands, however rising somethimes requires large rise commands.
aggressiveness: multiplied to PI-component of all commands sent. Low values lead to the drone flying "slower".
TODO
TODO
The behavior of the autopilot is set by sending commands on /uga_tum_ardrone/com of the form "c COMMAND". A Queue of commands is kept, and as soon as one command is finished (for example a way point reached), the next command is popped. The queue can be cleared by sending "c clearCommands". Commands can be sent using the drone_gui node. Some example scripts can be found in /flightPlans/*.txt. Possible commands are:
autoInit [int moveTimeMS] [int waitTimeMS] [int riseTimeMs] [float initSpeed]
takes control, starts drone, initializes PTAM. That is:
- starts the drone & and waits riseTimeMs, the drone will rise to approx.
a height of 1m.
- initializes PTAM by taking the first KF, flying up (sending initSpeed as control command) for moveTimeMS,
waiting waitTimeMS and then taking the second KF.
This is done until success (flying up and down respectively).
- good default values are "autoInit 500 800 4000 0.5"
setInitialYaw [double yaw]
sets the yaw the drone is started at. Most useful with the ardrone 2.0
with absolute yaw control, as it allows you to specify some other yaw
as the zero point rather than due north.
autoTakeover [int moveTimeMS] [int waitTimeMS] [int riseTimeMs] [float initSpeed]
takes control, initializes PTAM. The same as autoInit ...,
but to be used when the drone is already flying (skipps the first step).
takeoff
- takes control, starts drone.
- does not reset map or initialize PTAM
setReference [doube x] [double y] [double z] [double yaw]
sets reference point (used in goto X X X X).
setMaxControl [double cap = 1.0]
maximal control sent is capped at [cap], causing the drone to fly slower.
setInitialReachDist [double dist = 0.2]
drone has to come this close to a way point initially
setStayWithinDist [double dist = 0.5]
drone has to stay this close to a way point for a certain amount of time.
setStayTime [double seconds = 2.0]
time the drone has to stay within [stayWithinDist] for target to be reached.
clearCommands
clears all targets, such that the next command is executed immediately.
goto [doube x] [double y] [double z] [double yaw]
flies to position (x,y,z yaw), relative to current reference point.
blocks until target is reached according to set parameters
moveBy [doube x] [double y] [double z] [double yaw]
moves by (x,y,z,yaw), relative to the current target position blocks until target is reached according to set parameters
moveByRel [doube x] [double y] [double z] [double yaw]
moves by (x,y,z,yaw), relative to the current estimated position of the drone blocks until target is reached according to set parameters
land
initializes landing (use auto-land of drone)
lockScaleFP
sets the one point that does not change when the scale is re-estimated to the current drone position. The scaleFP can only be set when PTAM is good, i.e. this is delayed until PTAM is initialized and good. Need to set useWorldFixpoint in dynammic_reconfigure.
This node offers a simple QT GUI to control the drone_autopilot node, the drone_stateestimation node and fly the drone manually via keyboard or joystick.
DroneIP: IP address to periodically ping in order to estimate the latency of drone control. DO NO IGNORE THIS PARAMETER, it significantly affects performance.
KBsensRP: Sensitivity for roll/pitch Keyboard control
KBsensYaw: Sensitivity for yaw Keyboard control
KBsensGAZ: Sensitivity for gaz Keyboard control
None
None
On the top-right, the current publish-frequency of important topics is displayed:
The current control source has to be set (i.e. joystick or KB). The autopilot is only allowed to send control commands, if this is set to autopilot.
Joystick control requires a connected joystick and running "rosrun joy joy_node". We use a PS3 sixaxis controller. to make the controller work, a small linux-hack is required (set controller rights).
KB control requires the GUI window to have focus, but NOT the upper-left text field. => make the GUI the active window and press ESC to immediately enable KB control and set focus such that KB control works properly. can be used for safety (autopilot does wired stuff -> press ESC and immediately take over, disabling the autopilot and enabeling manual control).
Buttons Land, Takeoff, ToggleCam, Emergency (toggles emergency state).
calibrate with ethzasl_ptam. to work with colored images, in src/CameraCalibrator.cc change:
change function imageCallback(...) to
void CameraCalibrator::imageCallback(const sensor_msgs::ImageConstPtr & img)
{
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
if(mCurrentImage.size().x != img->width || mCurrentImage.size().y != img->height)
mCurrentImage.resize(CVD::ImageRef(img->width, img->height));
memcpy(mCurrentImage.data(),cv_ptr->image.data,img->width * img->height);
mNewImage = true;
}
can be estimated easily by
approximate in "simulation" based on c1 to c8:
The major part of this software package - that is everything except PTAM - is licensed under the GNU General Public License Version 3 (GPLv3), see http://www.gnu.org/licenses/gpl.html. PTAM (comprised of all files in /src/stateestimation/PTAM) has it's own licence, see http://www.robots.ox.ac.uk/~gk/PTAM/download.html. This licence in particular prohibits commercial use of the software.