ardrone_autonomy
ardrone_autonomy copied to clipboard
AutonomyLab
Using Multiple Instances of ardrone_autonomy
I am curently working to get multiple ardrones flying at the same time from a single desktop. I've built a simple launch file to create an instance of ardrone_autonomy in a namespace called "quad1". I also pass in ip arguments and a statement to handle which controller each drone is to be commanded from. My question is this: Even with compiling a second copy of ardrone_driver and attempting to run it along side the first instance of ardrone_autonomy, the second ardrone_driver fails. Does anyone know a way to resolve this error?
I see this error message:
[quad1/ARdrone-1] restarting process process[quad1/ARdrone-1]: started with pid [17386] Using custom ip address 192.168.1.10 Wait authentification Wait authentification Wait authentification ===================+> 192.168.1.10 [ INFO] [1360280789.946051099]: Root Frame is: 0 [ INFO] [1360280789.947098353]: ~cov/imu_la set to [0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1] [ INFO] [1360280789.948039719]: ~cov/imu_av set to [1, 0, 0, 0, 1, 0, 0, 0, 1] [ INFO] [1360280789.949083298]: ~cov/imu_or set to [1, 0, 0, 0, 1, 0, 0, 0, 100000] [ WARN] [1360280789.949989201]: Automatic IMU Caliberation is active. [ INFO] [1360280789.950096522]: using default calibration URL [ INFO] [1360280789.950166558]: camera calibration URL: file:///home/parcon/.ros/camera_info/ardrone_front.yaml [ INFO] [1360280789.953101752]: using default calibration URL [ INFO] [1360280789.953193572]: camera calibration URL: file:///home/parcon/.ros/camera_info/ardrone_bottom.yaml Deleting Profile -cea48d22 Deleting Application -e182b69b Input device Teleop added Starting thread video_stage Starting thread update_ros
video stage thread initialisation
Video multisocket : init 2 sockets
Video multisocket : connecting socket 0 on port 5555 UDP
Thread update_ros started
- Connection failed
Video multisocket : connecting socket 1 on port 5555 TCP
Failed to open AT
Starting thread navdata_update
Starting thread ardrone_control
VP_Com : Failed to open socket for navdata
PA : Start thread thread_academy_download
Academy download stage resumed
MEMORY SPACE ALLOWED : 40 MB
Start thread thread_academy
Start thread thread_academy_upload
- Connection failed
video stage thread ended
Academy download stage paused [ INFO] [1360280795.015007096]: SEND: CAT_COMMON/altitude_max = 2500.000000 (DEFAULT = 3000.000000) [ INFO] [1360280795.015084296]: SEND: CAT_COMMON/enemy_colors = 2.000000 (DEFAULT = 1.000000) [ INFO] [1360280795.015169021]: SEND: CAT_APPLI/navdata_options = 268435456.000000 (DEFAULT = 65537.000000) [ INFO] [1360280795.015248827]: SEND: CAT_USER/euler_angle_max = 0.087000 (DEFAULT = 0.209440) [ INFO] [1360280795.015302954]: SEND: CAT_USER/control_yaw = 1.750000 (DEFAULT = 1.745329) [ INFO] [1360280795.015363624]: SEND: CAT_APPLI/bitrate = 4000.000000 (DEFAULT = 1000.000000) [ INFO] [1360280795.015419406]: SEND: CAT_SESSION/max_bitrate = 4000.000000 (DEFAULT = 1000.000000) [ INFO] [1360280795.015475310]: SEND: CAT_SESSION/detections_select_h = 32.000000 (DEFAULT = 0.000000) [quad1/ARdrone-1] process has died [pid 17386, exit code -11]. log files: /home/parcon/.ros/log/7240ab1e-717e-11e2-a955-1c6f65c6b0bc/quad1-ARdrone-1*.log respawning... [quad1/ARdrone-1] restarting process process[quad1/ARdrone-1]: started with pid [17607] Using custom ip address 192.168.1.10
I'm trying to do the same thing and get the same error. If I run them directly using rosrun (re-exporting the ROS-NAMESPACE variable) it also crashes, but displays it as a Segmentation Fault.
Hi guys, I've been messing around trying to get this to work too. I've managed to get it working but not completely. I started just adding some good old fashioned print statements to figure out what was causing the second ardrone_driver to crash.
I found that commenting out the shared_raw_navdata->navdata_demo.bat line in the initialization part of the main loop and just giving it a dummy value (e.g. 99 ):
ROS_INFO("Successfully connected to '%s' (AR-Drone %d.0 - Firmware: %s) - Battery(\%): %d",
ardrone_control_config.ardrone_name,
(IS_ARDRONE1) ? 1 : 2,
ardrone_control_config.num_version_soft,
/*shared_raw_navdata->navdata_demo.vbat_flying_percentage*/99);
And setting the realtime_navdata parameter to true for the second driver works up to a point. I can now run two drivers at the same time and am getting video back from both drones but I'm only getting navdata from the first (unaltered) driver.
I'll keep working on it but maybe someone with a little more experience with C++ (I've only been using it for a few months) and the drone navdata could chime in on this?
Looked into this a bit further and the problem seems to be with opening UDP ports as the second driver fails to open the ports for both NavData and AT commands. So unfortunately it looks like a bug within the SDK when I have more time I'll get all the details together and post this as an issue on the Parrot boards. In the meantime though I've been looking at the work of Jaroslav Halgašík, he's managed to get control of multiple drones from a single computer (without video). His blog has more details (warning it's in Czech):
http://yerrix.blogspot.cz/search/label/AR.Drone
Alternatively they've written a conference paper on the work (in english):
http://aa4cc.dce.fel.cvut.cz/sites/default/files/downloads/publications/ardrone-ecc2013.pdf
So perhaps for multi-drone support we could forget about the sdk and just handle the navdata and at commands in the driver?
Hi everybody! Just wondering if there has been any progress on this issue? Mani Monajjemi was kind enough to point me to AR Drone Node.js where apparently they have control of several drones by the same machine. I tried linking ardrone_autonomy with the latest ARDroneLib files but got the same result - first drone works, second cannot launch. I would still prefer an integrated ROS solution over node.js. Thanks!
Any progress on this? Has anyone had any success flying multiple drones with ROS driver?
I tried recompiling with the latest version of the ARDrone SDK but it did not help. Since then I have been controlling using one computer per ARDrone. On Apr 19, 2013 4:02 AM, "pusnik" [email protected] wrote:
Any progress on this? Has anyone had any success flying multiple drones with ROS driver?
— Reply to this email directly or view it on GitHubhttps://github.com/AutonomyLab/ardrone_autonomy/issues/56#issuecomment-16640693 .
I have some great news regarding this issue. I am now able to fly multiple drones with 1 PC. The issue is in the ArDrone SDK in the file vp_com_socket.c around line 100. The problem is when the second drone node tries to bind client's IP (PC) with the socket, so that the drone can constantly send navdata to it. Here the error is received, and node crashes. Binding cannot be done, because the socket is already taken and binded by the first drone. So there is no need to bind it again.
This is the quick and ugly fix that should get multiple drones running. I already notified Parrot so I hope that they will fix this issue soon in the SDK.
case VP_COM_SERVER: name.sin_addr.s_addr = INADDR_ANY; int bind_err = bind( s, (struct sockaddr*)&name, sizeof(struct sockaddr)); if (bind_err < 0 ){ //res = VP_COM_ERROR; res = VP_COM_OK; }
More testing will be done on friday. I will post some result.
@pusnik That is a really good news.Please update this thread with your new results.
I performed a simple testing controlling 2 drones from 1 pc. Reading navdata and publishing commands worked perfectly. I didn't try if the video feed is working from both, but I expect it does.
I can also confirm this fix works. Drove two drones under continuous cmd_vel control simultaneously
I can't get this to work on my Ubuntu Fuerte setup - still reports the original error message. I've changed that file, deleted old builds in Soft/Build/targets_versions, and rerun build_sdk and rosmake. Am I missing something? Thanks.
I am guessing the updated file didn't compile. Check if you really compiled it. Simple printf statements in modified file should do it.
Yup, you're right - a fresh checkout did the trick. Video feeds work great too. Thank you for your help!
hi i'm new in this and i don't know to make the change that you do. i'm using the package of ardrone_autonomy and i can't connect to parrot because this make a mistake when i connect the second drone
Hi All, I'm curious to know if any further work has been done on this. After applying the advice given above to bba002865b, I was getting negative results on two quads joined to an access point. The first quad would come up fine (control, video, and navdata) running ardrone_driver with vanilla parameters. Then, running a second instance using the '-ip' parameter, I would get full connectivity to the second quad but lose connectivity to the first quad. Perhaps the previously-posted solution no longer works in light of more recent SDK versions?
I've done some further analysis and put together a solution based on UDP port remapping that appears to work fine for control, navdata, and video for multiple quads (tested on up to four v1.0's at once); my notes are posted here:
http://hacked10bits.blogspot.com/2014/02/multiple-ardrones-from-single-computer.html
Anyway, I've reached out to a couple people on this thread individually for feedback, but wanted to find out from the larger user community if anyone is having success using another approach. If not, is there interest in contributing this back into master (or groovy-devel, etc)? Cheers!
This is on my todo list too. My solution is like you mention at the end, a small, ROS enabled UDP reverse proxy program rather than iptables. That way root isn't required, which is a must in our setup.
I really like the proxy idea; do you have a working version? Since the customization of ports is needed in any case, I was going to put together a patch for the SDK this week or next and share it back with the group. What kind of ROS interaction did you have in mind for the proxy - would new instances of ardrone_driver publish/call to the proxy to inform it which ports to remap for a given IP? (We can also move this conversation elsewhere if preferred.)
Unfortunately no, just the rough notes. I'm swamped for at least the next two weeks and can't help until then, but the basic idea is that the udp proxy would have an interface over ros's dynamic reconfigure which would allow any ros program to request packets received from some IP at port P be forwarded to port X (and extra points for allowing a different forward IP too). The proxy would manage opening and closing ports as needed.
The ardronelib would then be modified to accept a pointer to the opened ports from ardrone_autonomy rather than open them itself. ardrone_autonomy would either open 5554-5 itself (the default, for single drone usage) or else open any available udp ports and tell udp_rev_proxy to do the appropriate forwarding. This would be configured by simply giving ardrone_autonomy the node path to the udp proxy (ie. when not specified use default behavior, when specified use the proxy).
I figured this would be a smaller, safer patch into ardronelib than attempting to have ardronelib use the ports it is commanded to by ardrone_autonomy, since there's no chance for port conflicts that would come with specifying a port on the command line. You could keep calling the same launch file over and over so long as you have just one udp proxy, only changing the ip address of the targeted drone without ever worrying about the local ports.
Great points; I think this will actually result in a slightly larger patch to ardronelib, but it sounds like the "right answer." It's officially on my todo list; will report back when I have something put together. If you free up before then, would be great to collaborate!
I have an existing github repository for ardronelib that could be helpful for this if you want to fork it.
I've spent a good chunk of yesterday and today looking into this further. Below is a first cut at a UDP proxy (technically a forward proxy) written in Python that relays between multiple local clients (i.e., ardrone_driver instances) and corresponding remote servers (i.e., drones). ROSification and incorporation of dynamic_reconfigure is work to be done still:
https://docs.google.com/uc?export=download&id=0B_uJH9qUbyTDR2RhekpBbm9TMW8
After looking more at ardronelib, it looks like switching to a proxy solution and using ardrone_driver-supplied ephemeral sockets will take a bit more work than I have time for at the moment. Getting VP_SDK to take pointers to externally-created sockets is going to take some doing. Here's another subtlety: in order to use any kind of proxy that operates using normal sockets, it is necessary for ardrone_driver instances to send their traffic to the proxy's IP rather than directly to the drone itself. Since the communications between the driver and the drone consist of both UDP (which must be fixed for multiple drones) and TCP (which is fine as-is), it would be better for the TCP traffic to continue to go directly between the driver and drone. Hence, simply setting the "-ip" argument won't suffice - it may be necessary to set multiple IP address arguments for each ardrone_driver instance.
One approach, sans ephemeral ports, might be to introduce new char* variables such as "udp_local_address" (likely a loopback IP - see the UDP proxy code for details) and "udp_proxy_address" inside the SDK and expose them as options (say, "-udp-local-ip" and "-udp-proxy-ip", respectively). I think the "quick" hack would be to modify ARDroneLib/VP_SDK/VP_Com/vp_com_socket.c at around line 86:
case VP_COM_SERVER:
name.sin_addr.s_addr = INADDR_ANY;
if ( sck->protocol == VP_COM_UDP && udp_local_address )
name.sin_addr.s_addr = inet_addr(udp_local_address)
sck->scn = inet_addr(udp_proxy_address)
if ( bind( s, (struct sockaddr*)&name, sizeof(struct sockaddr)) < 0 )
res = VP_COM_ERROR;
However, I suspect someone who has been working more heavily with the SDK can find a cleaner way to do it. By the way, I believe the ephemeral port solution would obviate the need for specifying the local IP but not the UDP proxy IP.
All that said, I offer the above script as a starting point (I have a halfway-complete version in C as well, if performance is a concern) and am still happy to create a patch for manual port customization if anyone is interested in going that route (the two approaches are actually complementary). Kenneth - let me know if you want to collaborate on this down the road when we both have more time. Cheers!
I'm back and ready to tackle this. Your patch, Mike, gives me an idea. If the port numbers for video and navdata were set to 0 then the existing code would open up an ephemeral port. If ardrone_autonomy could know which port was opened, it could hand this port number off to the proxy along with the IPs to forward from and to.
This should be as simple as moving navdata_socket in ardrone_navdata_client.c and icc_udp in video_stage.c to their respective .h files, then importing the appropriate files into ardrone_autonomy.
I'm going to try developing it this weekend using your patch as a starting point. I definitely see the value in manually specifying the ports on the command line, but I also want a turnkey solution with a ros-enabled UDP reverse proxy.
Sounds good, Kenneth! As you're probably aware, getsockname() can help with determining the ephemeral port assigned during bind(). A good, concise example is here: http://stackoverflow.com/questions/6666095/udp-client-source-port-in-c
Manual port specification works out well for our lab, but I agree with you that a "turnkey" solution would be great to have! Let me know how I can help!
After going through the trouble of getting the reverse udp proxy to work as I needed, it turns out that the drones don't care what ports the laptops open anyway. As far as I can tell, the entire issue preventing multiple drones from working is just bad programming. If a random, available port is opened instead of 5554-6 AND the vp_com_socket_t.remotePort is set to 5554-6, respectively for each of the three ports, the drones work perfectly fine. No proxies or port rewriting needed, the udp ports are only used for sending.
I've got a patch set which demonstrates this and incorporates Mike's patches in the hydro-devel branch of my fork of ardrone_autonomy https://github.com/kbogert/ardrone_autonomy/tree/hydro-devel . This works for me but the code is a bit ugly and I'll probably just make the modifications to parrot's lib and submit that for the pull instead (it's a small change).
I have not tested out anything other than flying two drones while watching their video feed and navdata, so please, if you're interested in this give the patch a good testing!
I do also have a completely working (mostly) turnkey patch with a udp-proxy, if people report issues with the current patch I'll post that next.
Nice work! So when the SDK sends probe datagrams to 5555 (video) and 5554 (navdata) on the drone, the drone must capture the probes' source UDP ports to use as its destination ports? Good thinking!
I've only had time to briefly look at your code changes; I look forward to giving it a whirl here. Though it might be nice for some users to still specify ports manually, if your solution meets everyone's needs then I'm fine with dropping the manual port spec code (though there shouldn't be any harm in keeping it). By the by, is use of local ephemeral ports a runtime option (thus preserving the default behavior of the SDK)?
It is not a runtime option at the moment, though could be. I'll make a minimal patch later on today.
Thank you very much @kbogert and @mikeclement for putting so much effort on this. I will go into ardrone development mode sometime next week and these patches are the first thing I will try and hopefully merge into the driver.
I just want to make sure: #98 and #99 are the final outcomes of this thread. right?
I'll let @kbogert confirm, but those should be the ones. He found a really elegant solution to the problem that doesn't require any iptables rules or runtime options, to my knowledge.
Related but unrelated, would you like me to add the Python script that scans for and reconfigures drones' wireless settings? If so, just let me know a preferred spot for it to live (it isn't itself a ROS node, just a utility).
Those should be the only changes needed to get multiple ardrone_autonomy instances running, yes.
A note on building though, ExternalProject seems to be too conservative when deciding whether or not to rebuild the SDK when it's using the .tgz file as source, if you just change the patch it won't do it. During development I used a git repository so I wasn't affected by this and forgot about it when I made the patches.
If you just apply the patches as they are catkin_make probably won't rebuild the SDK. A quick test shows you can force it to rebuild by renaming the .tgz (to ardrone-sdk-stripped-2.0.1_a.tgz , for example). I don't know how the build farm works, but this may be an issue with it, and will at least be an issue for anyone building from source.
@kbogert @mikeclement Thanks for clarifications and the note. The build farm will do the build inside a vanilla chrooted environment, so the new changes will be compiled. I will add some notes to the README about how to force recompilation of the SDK either using catkin_make clean or by removing the build and/or the devel folder. I should examine myself.