AP_LocationDB: add implementation of location database #24429

shiv-tyagi · 2023-07-29T05:57:47Z

This adds a global location database which can be used to store the location (as well other information like velocity, acceleration etc.) of other vehicles in neighborhood of a vehicle. This serves as a central repository for all such information and can be used for many purposes like following vehicles, tracking vehicles, avoiding vehicles etc.
Items can be added to this database from mavlink, ADSB and scripting as sources. This can be extended in future to support more sources. This has been tested thoroughly on SITL and units test for this have also been added to make sure this doesn't break in future.
This is used to implement multicopter avoidance to avoid collisions with neighboring vehicles around a copter.
More about the idea of implementing this database and its use cases are posted in this blog post.

Should be merged after #24508

peterbarker

Looking forward to seeing changes which insert data...

peterbarker · 2023-08-03T08:05:34Z

libraries/AP_Filesystem/AP_Filesystem_LocationDB.cpp

+        return -1;
+    }
+
+    bool readonly = ((flags & O_ACCMODE) == O_RDONLY);


Suggested change

bool readonly = ((flags & O_ACCMODE) == O_RDONLY);

const bool readonly = ((flags & O_ACCMODE) == O_RDONLY);

Should this be checking only to see if the O_RDONLY bit is present? i.e. flags & O_RDONLY

I think yes, we can do this but I followed AP_Filesystem_Mission. Also, what if more bits like O_WRONLY are also set? I mean, it might be due to an error but just in case.

libraries/AP_Filesystem/AP_Filesystem_LocationDB.cpp

peterbarker · 2023-08-03T08:10:15Z

libraries/AP_Filesystem/AP_Filesystem_LocationDB.cpp

+    const uint8_t item_size = sizeof(packet_t);
+    uint32_t total = 0;
+
+    while (count > 0) {


We need a sanity check here that we don't do too many oops.

Can't have anything that even remotely looks like an infinite loop in here.

I mean this looks right, but just a check that we don't stay in this loop more that 500 time would be good.

Okay. I have replaced it with a for loop with a limit on number of iterations.

libraries/AP_Filesystem/AP_Filesystem_LocationDB.cpp

libraries/AP_LocationDB/AP_LocationDB.h

peterbarker · 2023-08-03T09:01:36Z

libraries/AP_LocationDB/tests/test_database.cpp

+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX


Do we compile these tests for things other than those two?

libraries/AP_LocationDB/tests/test_database.cpp

peterbarker · 2023-08-03T09:03:35Z

libraries/AP_LocationDB/tests/test_database.cpp

+static float random_float(void) {
+    return ((((unsigned)random()) % 2000000) - 1.0e6) / 1.0e6;
+}


We have a HAL function to get random numbers, I believe.

But... we shouldn't be using random numbers in tests like this unless it is strictly required. Repeatability is key. other sorts of tests can monte-carlo all they like, but these unit tests shouldn't change run-to-run.

Which is to say you should be testing codepaths with crafted identifiers here, not relying on random numbers to explore the space.

Okay. I have removed the use of random numbers from the tests. Thanks for the suggestion.

peterbarker · 2023-08-03T09:06:50Z

libraries/AP_LocationDB/AP_LocationDB.cpp

+    return (uint8_t)KeyDomain::MAVLINK << 24 | sysid << 16 | compid << 8 | short_mav_msg_id(msgid);
+}
+
+// helper method to construct location database key for an adsb item


Suggested change

// helper method to construct location database key for an adsb item

#if AP_LOCATIONDB_KEYDOMAIN_ADSB_ENABLED

// helper method to construct location database key for an adsb item

... and in the configuration file you would

#ifndef AP_LOCATIONDB_KEYDOMAIN_ADSB_ENABLED #define AP_LOCATIONDB_KEYDOMAIN_ADSB_ENABLED HAL_ADSB_ENABLED #24377

(and include AP_ADSB_config.h in AP_LocationDB_config.h)

Similarly for the other domains.

Done for ADSB and SCRIPTING domains. Thanks. I couldn't find right define for MAVLINK domain though.

peterbarker · 2023-08-03T09:47:26Z

Current flash cost, sans scripting bindings which aren't working ATM:

Board               AP_Periph  blimp  bootloader  copter  heli  iofirmware  plane  rover  sub
Durandal                       2408   *           2288    2248              2136   2264   2240
HerePro             1688                                                                  
Hitec-Airspeed      *                                                                     
KakuteH7-bdshot                2120   *           2032    1992              2152   1944   1920
MatekF405                      2144   *           1968    1968              1984   2008   1992
Pixhawk1-1M-bdshot             2040               2048    1960              1976   1976   1936
f103-QiotekPeriph   *                                                                     
f303-Universal      *                                                                     
iomcu                                                           *                         
revo-mini                      2072   *           2200    2056              2096   1992   2024
skyviper-v2450                                    1840

Ryanf55

Pretty sweet to see this in action. A few minor requests, overall looking good. I was wondering - what's the behavior if vehicles are added to the database from both scripting and ADSB - do they conflict?

libraries/AP_LocationDB/AP_LocationDB.h

libraries/AP_LocationDB/AP_LocationDB.cpp

timtuxworth · 2023-08-06T04:20:15Z

libraries/AP_LocationDB/AP_LocationDB.h

+class AP_LocationDB {
+public:
+    friend class AP_Filesystem_LocationDB;
+    friend class AP_OABendyRuler;


I'm wondering why Bendy Ruler should be a friend class? Should we not try to avoid entangling the producer and consumer?

We do not give sequential access to the database elements to the outside world. To access any element, we need to have the key to the element. This works good in most of the cases. AP_OABendyRuler needs to access all the elements sequentially and it does not have keys to all the elements. Hence, I have made AP_OABendyRuler a friend class so that it can access the private method get_item_at_index

It would be better IMO to provide an iterator that can allow Bendy Ruler (or any consumer) to access the elements sequentially.

andyp1per

Refine enablement flag

andyp1per · 2023-08-06T17:53:53Z

libraries/AP_LocationDB/AP_LocationDB_config.h

+#include <AP_Scripting/AP_Scripting_config.h>
+
+#ifndef AP_LOCATIONDB_ENABLED
+#define AP_LOCATIONDB_ENABLED BOARD_FLASH_SIZE > 1024


This needs to be refined. Ideally it should be enabled based on the features that require it (OA at the moment?). Either way I think we should now avoid putting board size checks in the source code, but instead use whatever is the final result of @peterbarker 's minimal_config.inc

This needs to be refined. Ideally it should be enabled based on the features that require it (OA at the moment?). Either way I think we should now avoid putting board size checks in the source code, but instead use whatever is the final result of @peterbarker 's minimal_config.inc

I actually suggested this change - previously it was on on all boards, including 1MB boards.

We do have some 1MB boards which haven't been minimized yet, and I don't think we can use the minimize files.

I have wondered about how we might remove the BOARD_FLASH_SIZE checks throughout the code, but haven't really come up with any great ideas. We could add them in the chibios_hwdef.py in-line includes, I guess? In any case, I don't think the pattern I suggested here is uncommon in AP, so do you think we should hold things up here?

Why not make it dependent on the features that are using it? I think these are fairly specific

Also shouldn't below be:

#define AP_LOCATIONDB_KEYDOMAIN_ADSB_ENABLED HAL_ADSB_ENABLED && AP_LOCATIONDB_ENABLED

For example, otherwise switching it off is hard

Yep, the individual key domains would probably look something like this:

#define AP_LOCATIONDB_KEYDOMAIN_ADSB_ENABLED AP_LOCATIONDB_KEYDOMAIN_DEFAULT_ENABLED && HAL_ADSB_ENABLED

That's the way we're moving with a lot of our libraries.

We could potentially make it dependent on whether there's any consume of the data - could you investigate that, @shiv-tyagi , please?

Yep, the individual key domains would probably look something like this:

I have applied these changes. Thanks.

We could potentially make it dependent on whether there's any consume of the data - could you investigate that, @shiv-tyagi , please?

Yes, we can probably make it dependent on MODE_FOLLOW_ENABLED || AC_AVOID_ENABLED.

Update: I tried making AP_LOCATIONDB_ENABLED dependent on AP_FOLLOW_ENABLED (which is one of the consumers of the data) and it did not work out because AP_FOLLOW_ENABLED itself is dependent on AP_LOCATIONDB_ENABLED.

libraries/AP_LocationDB/tests/wscript

…athplanner

…NABLED

…ssages

timtuxworth · 2023-12-13T14:38:06Z

It would be great to get this PR in. What would it take?

shiv-tyagi · 2023-12-13T16:23:13Z

@timtuxworth I think we need to get #24624 merged first

magicrub · 2023-12-14T01:26:04Z

Why aren't we reusing or extending or refactoring or re-anythinging AC_Avoidance/AP_OADatabase? The word "Location" is kind of a key-word in ArduPilot and this object doesn't even contain that class! This new library has some new features like the FileSystem stuff but generally speaking it's a whole lot of redundant code re-inventing the wheel.

AP_LocationDB_Item:
    uint32_t _key;
    uint32_t _timestamp_ms;
    Vector3f _pos;
    Vector3f _vel;
    Vector3f _acc;
    float _heading;
    float _radius;
    uint8_t _flags;

AP_Avoidance::Obstacle:
        MAV_COLLISION_SRC src;
        uint32_t src_id;
        uint32_t timestamp_ms;

        Location _location;
        Vector3f _velocity;

        // fields relating to this being a threat.  These would be the reason to have a separate list of threats:
        MAV_COLLISION_THREAT_LEVEL threat_level;
        float closest_approach_xy; // metres
        float closest_approach_z; // metres
        float time_to_closest_approach; // seconds, 3D approach
        float distance_to_closest_approach; // metres, 3D
        uint32_t last_gcs_report_time; // millis

rmackay9 · 2023-12-14T02:01:04Z

I think the intention is for the new class to eventually replace the OA LocationDB so just need to be sure we do that in the long term at least we don't end up with both. So some kind of plan to get rid of the old one.

rmackay9 · 2024-04-10T04:15:00Z

I've looked at this again and I think that it is OK for the existing OALocationDB and this LocationDB to co-exist as long as we make it clear (perhaps in the comments or something) that their uses are different. This one is really for vehicles (manned or unmanned) while the existing LocationDB is for small obstacles.

shiv-tyagi force-pushed the pr-locdb branch 12 times, most recently from c375363 to 2dc7f33 Compare August 3, 2023 07:05

peterbarker requested changes Aug 3, 2023

View reviewed changes

Hwurzburg added the WikiNeeded needs wiki update label Aug 3, 2023

shiv-tyagi force-pushed the pr-locdb branch from 2dc7f33 to 372884e Compare August 4, 2023 12:47

IamPete1 mentioned this pull request Aug 4, 2023

AP_Scripting: generator: fix dependancy start and end miss-match #24508

Merged

shiv-tyagi force-pushed the pr-locdb branch from 372884e to c02de2f Compare August 5, 2023 14:30

shiv-tyagi marked this pull request as ready for review August 5, 2023 14:31

Ryanf55 requested changes Aug 6, 2023

View reviewed changes

shiv-tyagi force-pushed the pr-locdb branch from c02de2f to b1457c2 Compare August 6, 2023 04:12

timtuxworth reviewed Aug 6, 2023

View reviewed changes

shiv-tyagi force-pushed the pr-locdb branch 3 times, most recently from 00b0d66 to 4c7c4f1 Compare August 6, 2023 17:13

shiv-tyagi mentioned this pull request Aug 6, 2023

AC_Avoidance: avoid items in location database using BendyRuler #24517

Open

andyp1per requested changes Aug 6, 2023

View reviewed changes

Ryanf55 reviewed Aug 6, 2023

View reviewed changes

libraries/AP_LocationDB/tests/wscript Show resolved Hide resolved

shiv-tyagi force-pushed the pr-locdb branch 2 times, most recently from 8f96914 to 48848bd Compare August 7, 2023 18:20

shiv-tyagi added 22 commits August 30, 2023 08:25

AC_Avoid: add and use AC_AVOID_ENABLED

25e7f26

Copter: move AC_AVOID_ENABLED to AC_Avoid_config.h

59c9113

Copter: use AC_AVOID_ENABLED and AP_OAPATHPLANNER_ENABLED

4a464b2

AC_WPNav: use AC_AVOID_ENABLED

8c2abee

AP_Proximity: use AP_OAPATHPLANNER_ENABLED

a3e3611

AR_PosControl: use AC_AVOID_ENABLED

f6e8cc7

GCS_MAVLink: use AC_AVOID_ENABLED

ac1943d

RC_Channel: use AC_AVOID_ENABLED

e0bcb09

Rover: use AC_AVOID_ENABLED and AP_OAPATHPLANNER_ENABLED

319a7d7

AC_WPNav: create config file and make AC_WPNav_OA dependent on AP_OAP…

0a46659

…athplanner

AP_HAL: rename AC_OAPATHPLANNER_ENABLED to AP_OAPATHPLANNER_ENABLED

cbc11bb

AP_HAL_ChibiOS: rename AC_OAPATHPLANNER_ENABLED to AP_OAPATHPLANNER_E…

f2b1437

…NABLED

AP_LocationDB: first implementation of global location db

1c620fc

AP_LocationDB: add unit tests for location db

d4138b1

AP_Vehicle: add location database

61945cd

AP_Scripting: bindings and example for operations in AP_LocationDB

404fa81

AP_Filesystem: add filesystem backend for LocationDB

51e970b

GCS_MAVLINK: add handler for GLOBAL_POSITION_INT and FOLLOW_TARGET me…

3d59953

…ssages

AP_ADSB: push incoming vehicles to location db as well

7947b6f

Tools: add AP_LocationDB in build_options.py

2d93c47

Tools: add entry for AP_LocationDB in extract_features.py

534358a

Tools: disable AP_LocationDB for sitl_periph_gps

2c4eb2e

shiv-tyagi force-pushed the pr-locdb branch from 52a978d to 2c4eb2e Compare August 30, 2023 02:56

peterbarker mentioned this pull request Nov 2, 2023

No way to get current ROI from vehicle mavlink/mavlink#2049

Open

peterbarker mentioned this pull request Dec 13, 2023

[DRAFT] Add MSP Radar protocol support to Ardupilot #25684

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

AP_LocationDB: add implementation of location database #24429

AP_LocationDB: add implementation of location database #24429

shiv-tyagi commented Jul 29, 2023 •

edited

peterbarker left a comment

peterbarker Aug 3, 2023

peterbarker Aug 3, 2023

shiv-tyagi Aug 4, 2023

peterbarker Aug 3, 2023

shiv-tyagi Aug 4, 2023

peterbarker Aug 3, 2023

peterbarker Aug 3, 2023

shiv-tyagi Aug 4, 2023

peterbarker Aug 3, 2023

shiv-tyagi Aug 4, 2023

peterbarker commented Aug 3, 2023

Ryanf55 left a comment

timtuxworth Aug 6, 2023 •

edited

shiv-tyagi Aug 6, 2023 •

edited

timtuxworth Aug 6, 2023

andyp1per left a comment

andyp1per Aug 6, 2023

peterbarker Aug 7, 2023

andyp1per Aug 8, 2023

peterbarker Aug 9, 2023

shiv-tyagi Aug 9, 2023 •

edited

shiv-tyagi Aug 21, 2023

timtuxworth commented Dec 13, 2023

shiv-tyagi commented Dec 13, 2023

magicrub commented Dec 14, 2023

rmackay9 commented Dec 14, 2023

rmackay9 commented Apr 10, 2024

	bool readonly = ((flags & O_ACCMODE) == O_RDONLY);
	const bool readonly = ((flags & O_ACCMODE) == O_RDONLY);


		const AP_HAL::HAL& hal = AP_HAL::get_HAL();

		#if CONFIG_HAL_BOARD == HAL_BOARD_SITL \|\| CONFIG_HAL_BOARD == HAL_BOARD_LINUX

	// helper method to construct location database key for an adsb item
	#if AP_LOCATIONDB_KEYDOMAIN_ADSB_ENABLED
	// helper method to construct location database key for an adsb item

AP_LocationDB: add implementation of location database #24429

Are you sure you want to change the base?

AP_LocationDB: add implementation of location database #24429

Conversation

shiv-tyagi commented Jul 29, 2023 • edited

peterbarker left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

peterbarker commented Aug 3, 2023

Ryanf55 left a comment

Choose a reason for hiding this comment

timtuxworth Aug 6, 2023 • edited

Choose a reason for hiding this comment

shiv-tyagi Aug 6, 2023 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

andyp1per left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

shiv-tyagi Aug 9, 2023 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

timtuxworth commented Dec 13, 2023

shiv-tyagi commented Dec 13, 2023

magicrub commented Dec 14, 2023

rmackay9 commented Dec 14, 2023

rmackay9 commented Apr 10, 2024

shiv-tyagi commented Jul 29, 2023 •

edited

timtuxworth Aug 6, 2023 •

edited

shiv-tyagi Aug 6, 2023 •

edited

shiv-tyagi Aug 9, 2023 •

edited