LOS passing WP

[krisgry]

Hi, We are having a LOS problem that I would like to discuss with you to find the best solution.

The problem arises when two WPs are close together, the time of arrival factor is relatively large, and the UAV has to make a sharp turn, see the attached figure. 1) The UAV heads towards WP1 2) Upon the switch to WP2, the angle of the line WP1->WP2 is set as the desired heading 3) If WP2 is so close to WP1 that the UAV can not reach the line before WP2, it will along the line "forever" and never switch to WP3

So; How can we fix this? First its a matter of detection:

• Currently, we are detecting this in LOSnSMC by checking that we have passed the current WP (track_pos.x - track_length is larger than some threshold) and the course error is small.
• However, perhaps it should be made more general by e.g. adding a detection in PathController::updateTrackingState to check for an increasing ETA (over some period of time)?

Then its a matter of handling the problem:

• Currently, we do a signalError which leaves the UAV in an error state
• At least in the specific case shown in the picture it would be more intuitive to do a signalCompletion and move on to WP3
• For some more critical cases it might be necessary to "try again", but I have not looked into this

Looking forward for input on this! I have already discussed this with @jffortuna and @krisklau. I'll happily to a pull request.

[josebraga]

Hi,

The red line is what the UAV actually executes? At that point, does it consider WP2 and WP3 as attained ? I do not understand why it does not circle back to WP2 ..

I didn't fully understood this part: it will along the line "forever"

Can you provide a couple of logs so that we can take a look at them? Also, if possible, please provide the path you believe the vehicle should be doing in this situation. Should he go straight from WP1 to WP3 then? You also say "at least in the specific case..more intuitive" -- do you have other cases where it would make more sense to signal error?

The only thing that I am not very comfortable with, is to, in essence, add dynamic information (or basic models) about the system at the path controller level, when it's been agnostic for several years now and used in a lot of different systems. Adding turn ratio and threshold constraints plus checks make sense for fixed-wing UAV and AUVs, but not so for copters, ROVs or differential ASVs, and it would be hard for us now to test in all platforms -- of course, if your logs show us there's a complete disruption in the executed motion, then we must fix it somewhere.

Finally, bear in mind, I have done small amount of work at these layers, so it's possible my observations are not accurate :)

[mariacosta]

Hi krisgry,

I can't seem to replicate the same behaviour in my setup (tryed with LOS and LOSnSMC). In both cases, when forcing WP switch before "alignment", what happens is that when the UAV reaches WP1, it switches to WP2 and almost immediately jumps to WP3 (an then goes to WP3).

Can you share your path control configurations, so I can try to replicate the issue? I'm using the Simulation profile, by the way.

Regarding the items you put up for discussion:

• As for detection, the method you're currently using within the LOSnSMC task seems to be enough and for me it makes sense to remain only there;
• As to handling the problem, I would prefer for the controller to acknowledge the WP completion (even if it happens before "aligning") and thus move to the next WP (2nd option you suggest).

PS: One can argue that if the "WP2 is so close to WP1 that the UAV can not reach the line before WP2" then the plan definition is not suited for the characteristics of the platform being used, which implies changing the plan or to use another platform that can perform the desired plan. Having said that, the report you made is still a bug and should be fixed :)

[krisgry]

Dear Jose and Maria, Thanks for you quick and useful feedback.

I have reproduced the scenario in the attached logs. I have reproduced it using the LSTS version corresponding to SHA 35afba312393e1a5e8c0bd0558602871bd34a74b and with our current USER-space files, using AP-SIL. Hopefully this will still illustrate the problem. If not; let me know, and I'll make another attempt at running this from the LSTS master without a USER folder. 162926_pl_LOStest_FBW.zip

In order to produce this, I set the time of arrival factor down to 1 second, but we have also experienced this during real flight (once, during LOS tuning).

To clearify what I meant; The ideal behaviour of the UAV in the figure is WP1->WP2->WP3..., but since it never reaches WP2 it follows the line WP1->WP2 until the user does some action to regain control (stop the plan, start a new plan etc).

The following Neptus picture shows the actual behaviour:

Jose: I definitely see your point of not modifying the PathController, since it is deployed in a lot of different systems. I do not completely understand what you mean by "add dynamic information". Also; off the top of my head, a monotonically increasing time of arrival seems like undesirable behaviour regardless of the platform. Do you agree? Regarding the "specific case...more intuitive"; the only scenario I can think of are in cluttered environments, and is a bit far-fetched: Say that there is some object to the left of WP3 in the first figure, making it undesirable/dangerous to turn right from where the red line meets the dashed line to head towards WP3.

Maria: If I understand you correctly, you prefer a solution local to LOSnSMC with a detection similar to what we have been using, but change signalError to signalCompletion?

Maria, I like your idea of checking the plan beforehand. I guess it is a different discussion entirely, but is there any support for this today? We had some other operational issues where a possible solution would be to add a "sanity check" to the plan, to see that all parameters are the same for all maneuvers, the same controllers are active for all the maneuvers etc.

Again; please let me know if the logs are insufficient, and I should make some more, based on LSTS master.

[josebraga]

Hi,

"but since it never reaches WP2 it follows the line WP1->WP2 until the user does some action to regain control" - I guess the correct is: but since it never reaches WP3 it follows the line WP2->WP1..

What I am a little puzzled with is: shouldn't the underlying controller make the UAV circle back to WP3? It seems that it sets a desired direction when the WP3 is triggered but then there's no reaction to attain it if it misses it. If I am understanding the bug correctly, this will happen whenever the UAV misses the waypoint (e.g: if it's pushed aside by a wind gust?)

About the specific points:

• "add dynamic information": I meant to consider a math model of the vehicles being controlled in a way that the controllers would take into account maneuverability constraints -- e.g: instead of turning right immediately after WP2, move a little bit forward to offset from it, then turn and thus attain the WP3 while still in the turn
• I believe you are referring to the eta fields of *ControlState messages -- I agree with your observation, but I believe they are mostly suggestive and shouldn't be used by the controllers (just looking at the current usage in the lsts code)

Regarding checking the plan beforehand (in neptus), I feel it could be a viable solution, but, the way and direction the lsts toolchain is growing (with ripples and other services), I believe the idea is to have planners and mission supervisors on the user-side as agnostic as possible -- so that it is the same thing to plan for a copter, a auv, a floater or whatever -- then, it would be on the platform level the expense of measuring how attainable the desired plan actually (at least without having to reroute all the time) and warn the user if something's not ok. One drawback would be for bundles of plans designed beforehand that will be triggered live for the first time and there's no time for redesign

But, going back to my first thought, I still do not understand why the controller doesn't simply make the UAV turn back because, if it did so, there would be no issue at all -- I'll try to reproduce this on the AUVs but I am quite positive the AUVs realign towards the WP

In Line154 of Control/UAV/LOS it seems flawed to me that the desired roll reference does not take into account the track_pos.x (which turns negative when the uav overshoots the reference):

*double desired_tr = - m_args.tr_gain (xla ts.track_vel.y + ts.track_pos.y ts.track_vel.x);**

[krisgry]

Hi, A few clarifications:

• when I was saying "...never reaches WP2" I was referring to the first figure (the drawing, not the Neptus screen shot). For Neptus you are correct in assuming that I mean WP3, yes.
• this was produced using Control/Path/LOSnSMC. I have not tried using Control/UAV/LOS. What LOS are you using for the AUVs? If you are unable to reproduce the bug, I can have a look at this LOS. I have no experience with AUVs, but I imagine that the small turn radius of an AUV make this bug difficult to reproduce (unless you limit it, of course).
• I was referring to TrackingState eta, which is used to trigger the arrival at a WP. See https://github.com/LSTS/dune/blob/35afba312393e1a5e8c0bd0558602871bd34a74b/src/DUNE/Control/PathController.cpp#L742 where a low eta is used to set the nearby. This again sets the nearby flag of PathControlState (FL_NEAR, L1018), which finally triggers signalCompletion() in e.g. GOTO https://github.com/LSTS/dune/blob/a016911e92eb7c96b590a652d11b1e230f1066a8/src/Maneuver/Multiplexer/Goto.hpp#L78-L79. So my idea was to modify the "L742 (in PathController.cpp)"-if to also trigger if eta has been increasing for some time.

I guess it boils down to whether this is a general problem with PathController, or if it is a specific problem that should be fixed "locally" in LOSnSMC.

[josebraga]

Sorry, my bad.

Again:

• why doesn't LOSnSMC use ts.track_pos.x ?
• I was referring to usage in inherited tasks, but either way,

Should it really signalCompletion if point is actually not attained ? Why not circle back and attain it ?

Again, I am not seeing the issue in the PathController. My only concern is if, there's some dead lock where the vehicle keeps circling around the waypoint, but never really reaching it. But never, in any way, it makes sense to me to just keep moving away from the point like what's been happening...

[josebraga]

But I have serious doubts we will do the modification you suggested - for the AUVs, we have no issue in the AUV circling back to attain a missed waypoint. Also, it may be that due to strong water current, a waypoint may not be attainable in one direction, but is in another, since it is vital for the waypoint controller to always point to the waypoint. If you apply what you suggest, we'd have the vehicle stop or abort, whenever it misses a waypoint, and that's not what we want.

A compromise could be to have an argument dictating this behavior

if (m_args.dont_move_away && eta_increasing) { signalCompletion/Error; }

[rbencatel]

I need to take a look at reason why the LOSnSMC (and maybe the LOS) is not switching waypoint when ts.track_pos.x becomes negative (or positive, not sure right now about the used direction). LOS should switch to a new waypoint once the vehicle passes through the line perpendicular to the path at the waypoint or before it.

The ETA might start increasing even in slow aircraft if the wind is too strong in a certain direction. Because of that I am not sure if the eta_increasing is the best trigger to solve this problem. I agree with Braga that ts.track_pos.x is the key variable. I don't think it need to be used in the control law (Line154 of Control/UAV/LOS), but it need to be used in the verification of the waypoint switching condition.

Later I will try to check more in depth what is causing this issue.

[krisgry]

As I mentioned in my initial post, we currently handle the problem in LOSnSMC by using ts.track_pos.x, like you suggest:

          if ((ts.track_pos.x - ts.track_length > 20) && (chi_err < 0.1))
          {// we are following the LOS, but have passed the WP
            signalError(DTR("passedWP divergence error"));
          }

We have not submitted a PR on this yet, since we are still investigating it (only SITL), and wanted to discuss it with you to see what the best solution is.

I definitely see the point about not signalCompletion on a false basis, and I would like to test something like the "circle back"-functionality you are describing. Have you implemented anything similar for AUV that I can look at, perhaps? I notice that I am getting into the edges of how far my PathController-knowledge goes (at this point), so please correct me where I'm wrong :) Does there exist any functionality to simply say "start over" to the current maneuver? (i.e. go to the current WP (the one we just missed), starting from the current position).

I agree that it makes more sense to consider track_pos.x than ETA, but at least for us it is important to also consider ts.track_length so you know that you have passed the WP.

[Also; I have noticed that the along track monitor is turned off for LOSnSMC (in fact; I did not know of its existence before recently checking the code, so I hope this has not affected our discussion), but now I have experimented a bit with it. To my understanding/testing, this does not help, because it only considers track_pos.x, and not track_length; when you pass the WP it will not trigger as long as you "progress" (regardless of direction) is greater than the minimum. From your configs it seems that you do not use it for LOSnSMC either, but it does not seem like you are using LOSnSMC much (?). Further, it seems like you use Control/UAV/LOS for you x8s with the standard settings for ATM (enabled, period = 15s, min_speed=0.25). Is this correct?]

My conclusion, this far, is

1. continue handling the problem in LOSnSMC, like in the above snippet (replacing all hardcoded limits with parameters, of course)
2. replace the signalError with a "circle back"-functionality, yet to be implemented.

I am very open for suggestions on how to do 2. in an elegant manner :)

[josebraga]

Hi,

Well, I guess most of this discussion stems from different views on how waypoints should be followed and are reached. For AUVs, we've considered a waypoint as attained, only when the AUV reaches near it. The waypoint follower (we've used VectorField) keeps dispatching DesiredHeading references to guide the AUV towards it. There's even a special condition -- but always moves towards that waypoint

For UAVs, I was unaware that the path controllers (at least the one referenced by @rbencatel) consider a waypoint as attained when

the vehicle passes through the line perpendicular to the path at the waypoint or before it

which is a different perspective -- which probably changes how this should be dealt with, and makes a lot of my remarks useless :D

What I like about the current PathController's eta is the simple assumption: distance to waypoint (no matter where it's pointed at) over current speed -- maybe the bug sits in what triggers a signalCompletion() ?

[krisgry]

Hello again, I have looked a bit further into this, had a discussion with our pilots, and have an implementation that I would like to discuss.

We would like to add an option to change the way a maneuver completion is signalled (i.e. how PathController sets eta). The current implementation, uses the absolute value of the error (errx and erry) to determine eta, see: https://github.com/LSTS/dune/blob/a24e9fbf5c5a7e08e6c2ac45640e8c3b78cee09c/src/DUNE/Control/PathController.cpp#L745-L748 (and I'd be happy to hear an explanation on how you ended up with this particular way to calculate eta) However, we have had some problems in very strong winds and running LOS guidance. For example; the UAV is headed east, but encounters a strong wind from the south. It drifts off the path so that it is further away from the WP than the TOA distance, and will then just keep on flying in the same direction (as previously discussed). Our pilots then see it as the best option to move on to the next WP. One way of implementing this is to add an option for a second way to calculate the eta. By setting eta = m_ts.track_length - m_ts.track_pos.x (i.e. a sign-preserving version of errx), the maneuver is completed when passing a line that is orthogonal to the path, placed at the WP (or strictly; m_time_factor before it). So the vehicle will signal completion of the maneuver, regardless of the crosstrack error. I have implemented this through a simple switch case, where the default case is the above code snippet. I know that you are not very eager to change the extensively tested PathController class, but I hope we can discuss this matter :)

Another option would be to provide an interface similar to hasExternalZ, to allow the child path controllers to define how completion of a maneuver should be signalled/eta should be calculated.

Looking forward to hearing your inputs on this!

[josebraga]

i prefer the virtual method solution.

[josebraga]

by the way, if there are other fields you feel should be updated differently then I believe we can have even more solutions like this. if everyone agrees, we can implement and then each child can override as they prefer.

i do not know why it is like it is, but I do know the PathController was written and used for a LONG time only for AUVs and ASVs.. for both, we always circle around back to the waypoint, and so ETA always reflects the time to arrive and only when NEAR maneuvers signal completion. UAVs are other beasts and I guess, this, is one of many legacy solutions that doesn't fit properly. keep 'em coming. :+1:

[krisklau]

The virtual method solution seems like a good idea, yeah. As I'm reading your comment josebraga, I'm thinking it might be a good idea to try to seperate PathController even further (sometime, not now) by trying to take out all vehicle-specific code, and have a sub class AUVPathController which retains the current implementation and open up for UAV-specifics from a new class UAVPathController. :)

[krisgry]

Finally managed to get around with implementing this :) Let me know what you think