miguzca commented 3 years ago

Dear all,

Sorry to bother you in advance. I have been trying to run the flight plans provided in the repo (i.e. flightplan.txt, flightplan1.txt) and I am trying to create a geofence by drawing it on the map (keep out fence), however I think I am doing something wrong because it is not working correctly (see video Issue_Icarous-Geofence2.mp4). Additionally, when I start a mission with flightplan.txt, for some reason, I do not know why, at some point it re-computes the path without any apparent motive (because there are not fences at all, see video Issue_Icarous.mp4). Finally, I would like to understand a little bit more the DAA scenario (see video Issue_Icarous-DAA.mp4), I would like to know if the resolutions to avoid the conflict are vertical (hence we cannot see the effect in the screen), where I can check what kind of actions were taken to avoid the conflict, and what the colors mean (specifically, what the yellow and the red circle means). I have followed Issue #62 but unfortunately I have not been able to solve the issues. Do you have any idea about what can be happening?.

Icarous_Issues.zip

Thank you very much in advance. Kind regards,

SweeWarman commented 3 years ago

Regarding Issue_Icarous.mp4: this seems like it is recomputing a path because of a flightplan deviation conflict. Probably caused by the arducopter switching to the next waypoint. The XTRKDEV parameter determines the deviation threshold. The default is 10 m. Increase this to say 50m. Ideally, you want this number to be larger than the capture radius criteria used in arducopter to determine waypoint arrival.
Regarding Issue_Icarous-Geofence2.mp4: flightplan2.txt is lacking a reference speed. Add a speed command as done here. Use 1m/s. When a reference speed is not defined in the flightplan, Icarous assumes the vehicle is using the speed defined in DEF_WP_SPEED. However, in your case, it doesn't seem like arducopter is flying at DEF_WP_SPEED (1m/s). The vehicle is traveling too fast and the leg length is too short making it difficult to react in time. Setting the speed to 1m/s in the flightplan should fix this.
Regarding Issue-Icarous-DAA.mp4: the default behavior defined by RES_TYPE (=0) is for speed resolutions. Set RES_TYPE (=2) for track(heading) resolutions. For the head on encounter you are testing, speed resolutions won't resolve the conflict.

These suggestions should hopefully fix all the issues. Please let me know if they don't help.

miguzca commented 3 years ago

Hi!,

Thanks a lot for your reply!.

I have followed your recommendations and I got the following results.

Regarding Issue_Icarous.mp4: Worked perfectly with the modifications you suggested!
Regarding Issue-Icarous-DAA.mp4: Worked perfectly with the modifications you suggested!.
Regarding Issue_Icarous-Geofence2.mp4: In fact I believe flightplan2.txt has already a speed reference QGC WPL 110 0 0 0 16 0.000000 0.000000 0.000000 0.000000 37.102177 -76.387207 5.100000 1 1 0 3 16 0.000000 0.000000 0.000000 0.000000 37.102192 -76.386940 5.000000 1 2 0 3 16 0.000000 0.000000 0.000000 0.000000 37.101851 -76.386263 5.000000 1 3 0 3 16 0.000000 0.000000 0.000000 0.000000 37.101486 -76.386867 5.000000 1 4 0 3 16 0.000000 0.000000 0.000000 0.000000 37.102177 -76.387207 5.000000 1

However the result was exactly the same. I have also set the DEF_WP_SPEED to 1.0 in icarous_default.param (and in spacing_default.param just in case :) but the issue persisted. I have then changed the positions of wps 2 and 3 in the flightplan2.txt (see attached video) to give more space to be able to place the fence, but it stills does not work. Do you have maybe another idea of what can be misconfigured?.

Icarous_Issue-Geofence3.zip

Thank you very much in advance!.

SweeWarman commented 3 years ago

flightplan2.txt does not have a speed reference. You need the 178 entry as shown here.
I failed to notice that in your original video (Issue_Icarous-Geofence2.mp4), the fence has the floor and ceiling both set to 0. Which practically means you don't have a fence. Geofences are 3-dimensional. Use a floor of -10m and a ceiling of 50m for the example you are using (Or set the ceiling appropriately based on the altitude your aircraft is flying at).

miguzca commented 3 years ago

Hi!,

Ah ok, I misunderstood your previous suggestion. I have added the 178 entry as shown in the link. Wrt the geofence you are right, it is in 3D (I remember that in sitl in the past this was not needed, if I am not mistaken, that is why I did not do it in the first place). I have added the floor and the ceiling this time and the geofence in fact works (i.e. it stops the aircraft), however the re-planning component is not activated, therefore the aircraft just stops in the geofence (as shown in the video). Any idea about what can be happening?

issu_icarous-Geofence5.zip

Thank you very much in advance!.

SweeWarman commented 3 years ago

Could you please send me the logs so that I can take a closer look? Logs should be created under the exe/cpu1/logs folder. I suggest you remove all the current logs and rerun the scenario again to produce the logs corresponding to your current scenario.

miguzca commented 3 years ago

Sure!, Here they are

Cognition-aircraft0-1625756137.957685.log Path-aircraft0-1625756137.991871.log

SweeWarman commented 3 years ago

I tested the scenario in our simulation framework and everything seems to be working as expected. See attached video. This could be related to arducopter SITL. Could you set the ARUDCOPTER_VELCMD macro to true and recompile and try again. Please let me know if this helps.

https://user-images.githubusercontent.com/7516618/124950998-ed30d180-dfe0-11eb-990a-01dcace6edb8.mp4

miguzca commented 3 years ago

Hi!,

I have set ARUDCOPTER_VELCMD macro to true and I have re compiled but I did not see any difference when I ran the scenario. I have tried a different flight plan (flightplan.txt) and it has a strange behavior (see attached video). Should I try something different than SITL? I would like to make it work with SITL but I do not know why it is not working for me.

https://user-images.githubusercontent.com/85629894/125032059-b4026b00-e08d-11eb-9cea-4bcc6b25d648.mp4

SweeWarman commented 3 years ago

I am using arducopter version 4.0.1-dev. (We haven't tested compatibility with the newest versions of arducopter). It seems like you are using 4.1.0-dev. I am not entirely sure if this version difference is contributing to latent issues resulting in this strange behavior for you. But could you please switch to 4.0.1-dev and try again.
Also, repeat the last test again (using flightplan.txt), but this time using the geofence provided in the repository. Use this file. You can upload this fence as geofence load /path/to/file.

Please let me know what the response looks like after the above changes.

miguzca commented 3 years ago

Hi!,

Indeed, I am using ArduCopter V4.1.0-dev (8e586bc6). I have tried to find the version 4.0.1-dev but I have just found this, https://github.com/ArduPilot/ardupilot/tree/Copter-4.0/ArduCopter, should I just replace the files in the ArduCopter folder?.
I have repeated last test using flightplan.txt and geofence2.xml, see attached video.

https://user-images.githubusercontent.com/85629894/125250019-18276800-e2f6-11eb-8e2c-df8fecd3f7ac.mp4

SweeWarman commented 3 years ago

Sorry, I meant to say that you should try flightplan.txt and geofence2.xml using 4.0.1 (as I've indicated before, it seems like ICAROUS is not compatible with arducopter 4.1).
Here is 4.0.1: https://github.com/ArduPilot/ardupilot/tree/Copter-4.0.1. You can either git checkout this tag or directly download this version.

miguzca commented 3 years ago

Hi!,

Thank you for the feedback. I have switched to 4.0.1 (as shown in the video) and I ran the scenario with flightplan to and the geofence that I have tried before (the one you have in this comment https://github.com/nasa/icarous/issues/82#issuecomment-876542038) and it is still not working for me (the aircraft gets stuck in the fence). Additionally, I ran the scenario you mentioned (flightplan.txt and geofence2.xml), and you can see the result in the attached video. I am out of ideas of what could be wrong with my setting...

https://user-images.githubusercontent.com/85629894/125477736-0a096663-38d0-4722-a8b9-db2d1165a173.mp4

SweeWarman commented 3 years ago

Please send me the Cognition and Path logs from the last run. Let me see if I can spot any issues from the logs.
Meanwhile, you can also try out the python simulation framework to verify that the core geofence functionality is working. https://github.com/nasa/icarous/tree/master/Python/pycarous. You can follow the README to get started with the simulation. RunPySim.py will help you simulate flightplan.txt. The data contains example flight plans and fences you can use.

miguzca commented 3 years ago

Hi,

Sure, here are the logs. They do not show that much Cognition-aircraft0-1626191307.532900.log Path-aircraft0-1626191307.567757.log

I will try also with the python simulation framework. Thanks !

SweeWarman commented 3 years ago

We pushed a few updates and bug fixes recently, please make sure you get these changes as well.. not sure if any of these fixes help you... but worth a try.

SweeWarman commented 3 years ago

Also noticed from the logs that the aircraft is flying at 8 knots. The scenario with flightplan.txt/geofence2.xml was intended for 2 knots (1m/s). Could you please make sure that the mission speed is set to 1 m/s in the flightplan and also the DEF_WP_SPEED parameter in Icarous. Also please ensure the WPNAV SPEED on arducopter is also set to 1m/s.
Worst case scenario, we can setup a google hangout to meet and debug in real-time over a screen share session.

miguzca commented 3 years ago

Hi!,

I have pulled the changes from Icarous repo, compiled the files and ran the scenarios, unfortunately with the same results.
I have double checked the flightplan.txt and it is with a speed of 1m/s QGC WPL 110 0 0 0 16 0.000000 0.000000 0.000000 0.000000 37.102177 -76.387207 5.000000 1 1 0 0 178 0.000000 1.000000 0.000000 0.000000 0 0 0.000000 1 2 0 3 16 0.000000 0.000000 0.000000 0.000000 37.102335 -76.387195 5.000000 1 3 0 3 16 0.000000 0.000000 0.000000 0.000000 37.102335 -76.386206 5.000000 1 4 0 3 16 0.000000 0.000000 0.000000 0.000000 37.102110 -76.386481 5.000000 1 5 0 3 16 0.000000 0.000000 0.000000 0.000000 37.101796 -76.386859 5.000000 1 6 0 3 16 0.000000 0.000000 0.000000 0.000000 37.102187 -76.387287 5.000000 1

I have also checked icarous_default.param and it is set to 1 (see attached file)

DEF_WP_SPEED 1.0

I have modified WPNAV_SPEED to 1 (before it was 1000, is this the right place to change the parameter?) in copter-AVC2013.param (see attached file)

WPNAV_SPEED 1

Still, i have got the same results. Can you maybe provide me the files that you have in your setting that you believe are relevant for the configuration of the parameters in Icarous and arducopter?. I would like to compare your setting with mine to see if the issue is there.

Btw, I have changed the extensions of the files from param to log to be able to upload them here.

copter-AVC2013.log icarous_default.log

SweeWarman commented 3 years ago

Did you try the python sim? Could you please let me know if the geofence response worked with the python sim?

miguzca commented 3 years ago

Hi!,

Well, I have tried to follow the steps in the README file, but when I run

python3 setup.py build_ext --inplace

I get one error that I do not know how to fix,

`icarous/Python/pycarous$ python3 setup.py build_ext --inplace running build_ext building 'AutonomyStack' extension x86_64-linux-gnu-gcc -pthread -Wno-unused-result -Wsign-compare -DNDEBUG -g -fwrapv -O2 -Wall -g -fstack-protector-strong -Wformat -Werror=format-security -g -fwrapv -O2 -g -fstack-protector-strong -Wformat -Werror=format-security -Wdate-time -D_FORTIFY_SOURCE=2 -fPIC -I:/home/oliviero/icarous/Modules -I:/home/oliviero/icarous/Modules/Core/Cognition -I:/home/oliviero/icarous/Modules/Core/Guidance -I:/home/oliviero/icarous/Modules/Core/TrafficMonitor -I:/home/oliviero/icarous/Modules/Core/GeofenceMonitor -I:/home/oliviero/icarous/Modules/Core/TrajectoryManager -I:/home/oliviero/icarous/Modules/Core/TrajectoryManager/DubinsPlanner -I:/home/oliviero/icarous/Modules/Core/Interfaces -I:/home/oliviero/icarous/Modules/Core/Utils -I:/home/oliviero/icarous/Modules/Core/Merger -I:/home/oliviero/icarous/Modules/Core/EventManager -I/usr/include/python3.7m -c AutonomyStack.cpp -o build/temp.linux-x86_64-3.7/AutonomyStack.o AutonomyStack.cpp:671:10: fatal error: Commands.hpp: No such file or directory

include "Commands.hpp"

      ^~~~~~~~~~~~~~

compilation terminated. error: command 'x86_64-linux-gnu-gcc' failed with exit status 1 `

Which I do not know how to fix and I am not sure if I should expend time trying to figure that out as well.

SweeWarman commented 3 years ago

You need to define the LD_LIBRARY_PATH environment variable as instructed in the README under Modules/.

miguzca commented 3 years ago

I have done that already in the past, I have this in my ~/.bashrc file.

export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:${HOME}/icarous/Modules/lib

if I echo $LD_LIBRARY_PATH then I get this path

$ echo $LD_LIBRARY_PATH :/home/oliviero/icarous/Modules/lib:/home/oliviero/icarous/Modules/lib:/home/oliviero/icarous/Modules/lib

SweeWarman commented 3 years ago

These errors don't make much sense to me.. it could be related to the fact that you are using a virtual box environment to run linux (I don't know for sure). We can setup a meeting over hangout and discuss this. Let me know, and I can send you a meeting link

miguzca commented 3 years ago

Thank you very much for all the help!, I think I am going to use the python sim and we can then close the issue.

nasa / icarous

Issues running the examples (Geofence, flight plans) #82

include "Commands.hpp"