Bendy Ruler Lua Script Added #28903

TeteJK · 2024-12-18T18:19:43Z

Bendy Ruler Lua Script Implementation

Remaining issues:

needs to cover fence items (both inclusion and exclusion items)
need to split the GLOBAL_POSITION_INT and POSITION_TARGET_GLOBAL_INT targets
need to check against the old CUAV-OBC-2018 for behaviour
cleanup verbose messages like "Mission clear"
add logging to aid in understanding and debugging avoidance behaviour
document need for higher SCR_VM_I_COUNT and SCR_HEAP_SIZE
add autotest for CI, will need APIs for genobstacles when running headless
add airspace clear checking for takeoff with MAV_CMD_NAV_DELAY_AIRSPACE_CLEAR and for jumps with CONDITION_TYPE_AIRSPACE_CLEAR and CONDITION_TYPE_AIRSPACE_NOT_CLEAR

Problem framing from @NDevDrone and @tridge

This implementation aimed to integrate the Bendy Ruler obstacle avoidance algorithm into the latest version of ArduPilot by using a Lua script for evaluation and calculations.

1. Enhancements to the AP_Avoidance Module

Bindings and functions related to obstacle detection using ADS-B were implemented within the AP_Avoidance module to interact with Lua. The following methods were added:

num_obstacles()
Returns the total number of obstacles detected via ADS-B (requires ADS-B to be enabled).

get_obstacle_loc(instance)
Retrieves the last known position of a detected obstacle as a Location userdata type. The instance parameter specifies the obstacle.

get_obstacle_vel(instance)
Provides the velocity of a detected obstacle as a 3D vector (Vector3f) in NED (North-East-Down) coordinates, measured in meters per second.

get_obstacle_id(instance)
Returns the unique ID of an obstacle as reported by ADS-B, using the instance parameter.

get_obstacle_time(instance)
Returns the timestamp of the ADS-B message for a specific obstacle as an integer value.

These methods, accessible via the AP_Avoidance singleton instance, enable real-time interaction with ADS-B data. Where existing ArduPilot methods sufficed for Lua script interactions, they were reused. Missing or underperforming methods were hardcoded into the Lua script.

2. Lua Implementation of the Bendy Ruler Logic

The Bendy Ruler avoidance algorithm was implemented in Lua, with modifications to adapt to the scripting environment:

Exclusions: Logic for fences and margin areas was omitted for simplicity. The current focus is solely on avoiding obstacles.
Simplified Task Logic: Instead of using threads and schedulers, the avoidance logic is invoked within the update() function. This ensures computations only occur when a potential threat is detected, optimizing limited computational resources when working with Lua scripts.
Navigation Commands: The Lua script does not interact directly with navigation and command libraries. Instead, it sends commands to fly to a new position when avoidance is required using the already implemented Lua methods.

3. Addressing Altitude Handling Issues

The GenObstacles library, used for terrain altitude calculations, had a bug preventing accurate obstacle generation. To enable horizontal avoidance testing:

All altitude-dependent logic was temporarily commented out.
Obstacles were generated without considering altitude.

This workaround allows testing of horizontal avoidance logic but requires future fixes to handle altitude data correctly.

4. STIL Simulation for Testing the Lua Script

Follow these steps to test the Lua script implementation for the Bendy Ruler obstacle avoidance algorithm:

1. Prepare the Simulation Environment

Navigate to the directory where the simulation will be run.
Create a folder named scripts in this directory (if not already present).
Copy the latest version of the Lua script containing the Bendy Ruler code (Bendy_ruler.lua) into the scripts folder.

2. Start the Simulation

Run the simulation using the following command, replacing IP with the appropriate IP address:

python sim_vehicle.py -w -v ArduPlane -f plane-elevon --console --map -C --load-module asterix,genobstacles --out=udp:IP:14550

3. Set Bendy Ruler Parameters

param set SCR_ENABLE 1 
param set AVD_ENABLE 1 
param set ADSB_TYPE 1
param set SCR_VM_I_COUNT 1000000
param set AVD_W_ACTION 2 
param set AVD_W_DIST_XY 5000

4. Clear Initial Obstacles

Delete all obstacles generated during startup to avoid overloading the Lua script.

Right-click on the map.
Select Obstacles > Clear All.

5. Reset Bendy Ruler Lua Script

Something like

script stop bendy-ruler.lua
script load bendy-ruler.lua

allow lua scripts to access avoidance database

this is a port of the plane bendy ruler from the 2018 OBC https://github.com/tridge/ardupilot/tree/CUAV-OBC-2018

always maintain original target location

timtuxworth · 2024-12-22T04:59:51Z

libraries/AP_Scripting/applets/Bendy_ruler.lua

+
+        local avoid_dist = orig_target_loc:get_distance(new_target_loc)
+
+        gcs:send_named_float("AVD_DIST", avoid_dist)


What does this do?

timtuxworth · 2024-12-22T05:00:16Z

It would be really useful to have some logging.

timtuxworth · 2024-12-24T04:09:18Z

libraries/AP_Avoidance/AP_Avoidance.h

+    int32_t get_obstacle_time(uint8_t instance) const {
+        return _obstacles[instance].timestamp_ms;
+    }
+


I'm thinking it might be better to have this as a single method that returns all these values in a single call right @tpwrules ?
Apart from the simplicity and flash savings isn't there also the risk of a race condition?

timtuxworth · 2024-12-24T04:09:51Z

Tools/autotest/locations.txt

@@ -104,3 +104,5 @@ Egge=60.215720,10.324071,198,303
 Gundaroo=-35.02349196,149.26496411,576.8,0
 Kaga=36.3268982,136.3316638,44,0
 UCSB=34.413963,-119.848946,0,0
+California=37.4275449,-122.1697,0,0


California is a big place - just saying.

timtuxworth · 2024-12-24T04:11:33Z

libraries/AP_Scripting/docs/docs.lua

+-- Returns the number of detected obstacles via ADS-B
+-- the ADS-B Must be enabled.
+---@return integer -- number of obstacles
+function avoid:num_obstacles() end-- Returns a Location userdata for the last position of the obstacle observed.


The comment does't seem to match the function.

timtuxworth · 2024-12-24T04:12:51Z

libraries/AP_Scripting/docs/docs.lua

+
+---@param instance integer -- instance number
+---@return Vector3f_ud -- 3D velocity in m/s, in NED format
+function avoid:get_obstacle_vel(instance) end-- Obstacle id as obtained from ADS-B.


Can't this be other obstacles? Not just ADS-B? What about Remote ID vehicles or something identified by an off board AI/camera?

timtuxworth · 2024-12-24T04:14:43Z

libraries/AP_Scripting/applets/Bendy_ruler.lua

+-- MAV_COLLISION_SRC
+MAV_COLLISION_SRC = {
+    ADSB = 0,                         -- Source is ADSB_VEHICLE packets
+    MAVLINK_GPS_GLOBAL_INT = 1,        -- Source is MAVLink GPS_GLOBAL_INT


Remote ID?
Offboard AI?
Fixed obstacles like cell towers or power lines?

timtuxworth · 2024-12-24T04:23:03Z

libraries/AP_Scripting/applets/Bendy_ruler.lua

+}
+
+local PARAM_TABLE_KEY = 7
+local PARAM_TABLE_PREFIX = "BENDY_"


Just a suggestion, for parameter names, I've started using this convention:
prefix =
Z - identifies a scripting parameter
XX - scripting function - e.g. PF = Plane Follow - so BR = Bendy Ruler

ZPF_ Plane Follow parameters
ZTA_ Terrain Avoidance parameters
(possibly?) ZAL_ Auto Land parameters ...

so
ZBR_ for bendy ruler parameters

This means:

its easy to find any scripted parameters by just searching or "Z" (or Z* in mavproxy).

the maximum hit for bendy ruler (for up to 99 parameters) is 3 characters.

Which leaves room for coding some reasonable meaning into the remaining characters.

Where did you get the idea we can't have more than nine? A table can have 62 or so.

I hit this in writing quadplane_terrain_avoid.lua. An error definitely gets thrown if you try to add more than 10 parameters to a table. (I guess it's 10, not 9)

I think its in AP_Param.h - AP_PARAM_MAX_DYNAMIC = 10, even though there is

if (num_params > 63) { return false; }

at line 2999 of AP_Param.cpp, I think all the code in add_table() uses AP_PARAM_MAX_DYNAMIC as the limit, and I have found that you definitely can't add more than 10 parameters in a table.

Turns out I was wrong about the 9 parameter limit. Thanks for setting me straight on this.

timtuxworth · 2024-12-24T04:33:46Z

libraries/AP_Scripting/applets/Bendy_ruler.lua

+  // @Description: Avoidance margin for fence
+  // @User: Advanced
+--]]
+BENDY_MARGIN_FENCE = bind_add_param('MARGIN_FENCE', 2, 50)


How about MRG - jus thinking we might need the characters later so
MRG_FEN or MRG_FNC
MRG_DYN
MRG_EXC

timtuxworth · 2024-12-24T04:34:51Z

libraries/AP_Scripting/applets/Bendy_ruler.lua

@@ -0,0 +1,871 @@
+local UPDATE_RATE_HZ = 10


Aren't CAPITALs supposed to be reserved for GLOBALS?

It’s a global considering it’s in the outermost scope of the script and a setting at the top. Marking it as a local is probably marginally better for performance.

timtuxworth · 2024-12-24T04:36:28Z

libraries/AP_Scripting/applets/Bendy_ruler.lua

+end
+
+function longitude_scale(lat)
+    local DEG_TO_RAD = math.pi / 180  


there is already a math.rad() function in Lua

timtuxworth · 2024-12-24T04:41:29Z

libraries/AP_Scripting/applets/Bendy_ruler.lua

+    loc2:lat(lat)
+    loc2:lng(lon)
+    loc2:alt(loc1:alt())
+    --gcs:send_text(0, string.format("IN: Lat: %.2f, Lon: %.2f", loc1:lat(),loc1:lng()))


Need the frame

timtuxworth · 2024-12-24T04:42:45Z

libraries/AP_Scripting/applets/Bendy_ruler.lua

+  // @Param: BENDY_OPTIONS
+  // @DisplayName: ADS-B avoidance options
+  // @Description: Bitmask of behaviour options
+  // @Values: 0:Disabled,1:Enabled


If this is enabled/disabled it should be _ENABLE

timtuxworth · 2024-12-24T04:55:21Z

libraries/AP_Scripting/applets/Bendy_ruler.lua

+    if full_distance < 20 then
+        return nil
+    end    
+    -- Try 5 degree increments around a circle, alternating left and right. Check each one to see if flying in that direction would avoid all obstacles.


Seems to be 15 degrees (or am I reading it wrong)?
bearing_inc_cd = 1500

timtuxworth · 2024-12-24T05:04:51Z

libraries/AP_Scripting/applets/Bendy_ruler.lua

+    if new_target_loc then
+        -- tell the vehicle to fly to the new calculated target
+        vehicle:update_target_location(target_loc, new_target_loc)
+        last_updated_target_loc = new_target_loc:copy()


update_target_location() is probably going go fail sometimes, so maybe last_updated_target_loc should only get updated if it returns true?

timtuxworth · 2024-12-24T05:07:50Z

libraries/AP_Scripting/applets/Bendy_ruler.lua

+        gcs:send_named_float("AVD_DIST", avoid_dist)
+    else
+        -- revert to the original target location, no avoidance needed
+        vehicle:update_target_location(target_loc, orig_target_loc)


It's not clear why you are doing this. It seems like it would already be going here, so why tell it to go where it's already going?

timtuxworth · 2024-12-24T05:09:04Z

libraries/AP_Scripting/applets/Bendy_ruler.lua

+end
+
+local orig_target_loc = nil
+local last_updated_target_loc = nil


What is this for? It doesn't seem to get used anywhere.

Teresa Krings and others added 3 commits December 13, 2024 11:27

AP_Avoidance: add lua bindings

60c5cab

allow lua scripts to access avoidance database

Tools: added additional locations for BendyRuler testing

b410232

AP_Scripting: added avoidance bindings

94fbb7a

Hwurzburg added the WikiNeeded needs wiki update label Dec 18, 2024

AP_Scripting: added BendyRuler applet

65debec

this is a port of the plane bendy ruler from the 2018 OBC https://github.com/tridge/ardupilot/tree/CUAV-OBC-2018

tridge force-pushed the master branch from a79f24f to 65debec Compare December 19, 2024 21:05

tridge added WIP Plane VTOL-Plane Scripting labels Dec 19, 2024

AP_Scripting: cleanup Bendy_ruler vehicle target logic

37ac512

always maintain original target location

timtuxworth reviewed Dec 22, 2024

View reviewed changes

timtuxworth reviewed Dec 24, 2024

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Bendy Ruler Lua Script Added #28903

Bendy Ruler Lua Script Added #28903

TeteJK commented Dec 18, 2024 •

edited by tridge

Loading

timtuxworth Dec 22, 2024

timtuxworth commented Dec 22, 2024

timtuxworth Dec 24, 2024

timtuxworth Dec 24, 2024

timtuxworth Dec 24, 2024

timtuxworth Dec 24, 2024

timtuxworth Dec 24, 2024 •

edited

Loading

timtuxworth Dec 24, 2024 •

edited

Loading

tpwrules Dec 24, 2024

timtuxworth Dec 24, 2024 •

edited

Loading

timtuxworth Dec 24, 2024 •

edited

Loading

timtuxworth Jan 3, 2025

timtuxworth Dec 24, 2024 •

edited

Loading

timtuxworth Dec 24, 2024

tpwrules Dec 24, 2024

timtuxworth Dec 24, 2024 •

edited

Loading

timtuxworth Dec 24, 2024

timtuxworth Dec 24, 2024

timtuxworth Dec 24, 2024

timtuxworth Dec 24, 2024

timtuxworth Dec 24, 2024

timtuxworth Dec 24, 2024 •

edited

Loading


		local avoid_dist = orig_target_loc:get_distance(new_target_loc)

		gcs:send_named_float("AVD_DIST", avoid_dist)

Bendy Ruler Lua Script Added #28903

Are you sure you want to change the base?

Bendy Ruler Lua Script Added #28903

Conversation

TeteJK commented Dec 18, 2024 • edited by tridge Loading

Bendy Ruler Lua Script Implementation

1. Enhancements to the AP_Avoidance Module

2. Lua Implementation of the Bendy Ruler Logic

3. Addressing Altitude Handling Issues

4. STIL Simulation for Testing the Lua Script

1. Prepare the Simulation Environment

2. Start the Simulation

3. Set Bendy Ruler Parameters

4. Clear Initial Obstacles

5. Reset Bendy Ruler Lua Script

Choose a reason for hiding this comment

timtuxworth commented Dec 22, 2024

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

timtuxworth Dec 24, 2024 • edited Loading

Choose a reason for hiding this comment

timtuxworth Dec 24, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

timtuxworth Dec 24, 2024 • edited Loading

Choose a reason for hiding this comment

timtuxworth Dec 24, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

timtuxworth Dec 24, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

timtuxworth Dec 24, 2024 • edited Loading

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

timtuxworth Dec 24, 2024 • edited Loading

Choose a reason for hiding this comment

TeteJK commented Dec 18, 2024 •

edited by tridge

Loading

timtuxworth Dec 24, 2024 •

edited

Loading

timtuxworth Dec 24, 2024 •

edited

Loading

timtuxworth Dec 24, 2024 •

edited

Loading

timtuxworth Dec 24, 2024 •

edited

Loading

timtuxworth Dec 24, 2024 •

edited

Loading

timtuxworth Dec 24, 2024 •

edited

Loading

timtuxworth Dec 24, 2024 •

edited

Loading