I was playing with ChatGPT today to get me started down the path of LUA scripting, and in particular, solving for the triangles left by Bendy Ruler object avoidance.
The code below doesn’t run in the SITL simulation (fails on line 1 - could be file formatted with Notepad - will chase), but think of it as a concept and let me know your thoughts. It’s simplistic, but allows me to learn a bit.
Here’s some of the commentary from the AI:
Scenario:
- Waypoints:
- WP1: Located 100 meters south of WP2, so the path runs from south to north.
- WP2: Located directly north of WP1 by 100 meters.
- Tree:
- Located roughly in the center between WP1 and WP2.
- The tree trunk has a diameter of 0.2 meters, and the exclusion zone is circular with a 3-meter radius, centered around the tree.
Path Calculation:
- The mower starts at WP1 and moves north along the straight path towards WP2.
- When the mower gets within the 3-meter exclusion zone radius around the tree, the script determines whether the exclusion zone (tree) is on the left or right side of the path.
- Based on our assumption, the mower would divert right (clockwise), perform a semi-circle bypass around the exclusion zone, and rejoin the straight path towards WP2.
Path Geometry:
- The path consists of:
- A straight line from WP1 towards the point where the exclusion zone is detected (3 meters south of the tree’s center).
- A semi-circle diversion (with a 3-meter radius) around the tree.
- A straight line back to the original path, leading to WP2.
Let me now plot this based on your expectation of two straight line segments with a semi-circle in the middle.
Mower Path from WP1 to WP2 with Tree Exclusion Zone
<<<image didn’t come over - will fix/reply later>>>
Here is the plot of the mower’s path from WP1 to WP2 with the tree exclusion zone in the middle. The path consists of two straight line segments and a semi-circle in the middle to circumnavigate the tree. The mower moves along the straight line, detects the exclusion zone, diverts to the right in a clockwise semi-circle around the tree, and then resumes the straight path toward WP2.
I since added a slowdown when interpreting a fence, and a speed increase upon reaching the path, and some messages I hope will show up in MP.
Here’s the code:
-- Function to calculate distance between two GPS points (for fence proximity)
function distance(lat1, lon1, lat2, lon2)
local R = 6371000 -- Earth radius in meters
local dLat = math.rad(lat2 - lat1)
local dLon = math.rad(lat2 - lon1)
local a = math.sin(dLat / 2) * math.sin(dLat / 2) +
math.cos(math.rad(lat1)) * math.cos(math.rad(lat2)) *
math.sin(dLon / 2) * math.sin(dLon / 2)
local c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
return R * c -- Returns distance in meters
end
-- Function to calculate the cross product to determine left or right of path
function cross_product(v1_lon, v1_lat, v2_lon, v2_lat)
return (v1_lon * v2_lat) - (v1_lat * v2_lon)
end
-- Function to determine avoidance direction based on proximity to path
function determine_avoidance_direction(wp1_lat, wp1_lon, wp2_lat, wp2_lon, fence_lat, fence_lon)
-- Vector for the path between WP1 and WP2
local path_vector_lon = wp2_lon - wp1_lon
local path_vector_lat = wp2_lat - wp1_lat
-- Vector from WP1 to the exclusion fence center
local fence_vector_lon = fence_lon - wp1_lon
local fence_vector_lat = fence_lat - wp1_lat
-- Calculate the cross product
local cross = cross_product(path_vector_lon, path_vector_lat, fence_vector_lon, fence_vector_lat)
-- Determine avoidance direction
if cross > 0 then
-- Fence is on the left of the path; avoid by going right
gcs:send_text(6, "Exclusion fence on left, avoiding to the right.")
return "right"
else
-- Fence is on the right of the path; avoid by going left
gcs:send_text(6, "Exclusion fence on right, avoiding to the left.")
return "left"
end
end
-- Function to check fence proximity and execute avoidance
function check_fences_and_avoidance(wp1_lat, wp1_lon, wp2_lat, wp2_lon)
-- Get the current GPS coordinates of the rover
local current_lat = ahrs:get_latitude() -- Rover latitude
local current_lon = ahrs:get_longitude() -- Rover longitude
-- Ensure GPS is functioning correctly
if gps:status() < 3 then
gcs:send_text(6, "GPS fix is weak, cannot proceed with avoidance.")
return check_fences_and_avoidance, 1000 -- Retry after 1 second
end
-- Get the number of fence vertices (exclusion zones)
local fence_count = fence:vertex_count()
-- Variable to store the original speed before slowing down
local original_speed = param:get("CRUISE_SPEED")
-- Loop through all fence vertices
for i = 0, fence_count - 1 do
local fence_vertex = fence:vertex(i)
-- Get the lat, lon, and radius of the fence exclusion zone
local fence_lat = fence_vertex:lat()
local fence_lon = fence_vertex:lng()
local fence_radius = fence_vertex:radius() -- Radius in meters
-- Calculate the distance to the fence center
local dist_to_fence = distance(current_lat, current_lon, fence_lat, fence_lon)
-- Check if within 15 meters of the fence
if dist_to_fence < 15 and dist_to_fence >= fence_radius then
-- Send message and reduce speed by 50%
gcs:send_text(6, "Approaching exclusion fence, reducing speed by 50%")
param:set("CRUISE_SPEED", original_speed * 0.5) -- Reduce speed by 50%
end
-- Check if within the exclusion zone's radius
if dist_to_fence < fence_radius then
-- Determine the circumnavigation direction
local avoidance_direction = determine_avoidance_direction(wp1_lat, wp1_lon, wp2_lat, wp2_lon, fence_lat, fence_lon)
-- Avoidance logic based on direction
if avoidance_direction == "right" then
-- Implement right-side avoidance (circumnavigate clockwise)
vehicle:set_mode(HOLD) -- Temporarily stop to adjust the path
gcs:send_text(6, "Executing right-side circumnavigation.")
-- Add logic for right-side waypoint adjustment
elseif avoidance_direction == "left" then
-- Implement left-side avoidance (circumnavigate counterclockwise)
vehicle:set_mode(HOLD) -- Temporarily stop to adjust the path
gcs:send_text(6, "Executing left-side circumnavigation.")
-- Add logic for left-side waypoint adjustment
end
-- After avoidance, resume normal mission or AUTO mode
vehicle:set_mode(AUTO) -- Resume mission
-- Restore the original speed after avoidance
gcs:send_text(6, "Resuming original speed after avoiding the fence.")
param:set("CRUISE_SPEED", original_speed) -- Restore the original speed
end
end
-- Continue checking every second
return check_fences_and_avoidance, 1000
end
return check_fences_and_avoidance