User/rnar17/new update collision zones by rnar17 · Pull Request #822 · UBCSailbot/sailbot_workspace

rnar17 · 2026-02-23T04:09:59Z

Description

Resolves Updating the Boat Collision Zone's Size #458
Implemented triangular collision zone for boats with non‑zero ROT. Added collision zone case for if there is no collision possible between AIS ship and Sailbot.
Triangle is defined by:
- A: current bow position
- B: projected collision point at current heading
- C: projected collision point after applying ROT over a 5‑second horizon
Added rot_to_rad_per_sec() helper in coordinate systems since its logic is used in a few places

The boat on the left has ROT > 0, and _calculate_projected_distance() predicts a future intersection with the Sailbot. In this case, the collision zone is generated using the turning‑motion method described above, producing a triangular swept region. The region naturally fans outward as it extends away from the AIS ship. This is more realistic since a turning ship can sweep through a larger lateral area the farther out you go from its current position.

Verification

Created unit tests in test_obstacles.py that verify collision zone creation and geometry for no collision case, straight line case (ROT=0), and turning motion case (ROT>0)
Created unit tests in test_coord_systems.py for rot_to_rad_per_sec()

raghumanimehta

Thanks for the PR @rnar17 !! This task was indeed quite complicated.

I have pointed a couple of changes, and I have also requested a review on coordinate systems given PATH's troubles with it. I am in the process of updating the documentation in code and confluence. I will notify you when that's done.

Please use that to verify the coordinates on this PR. You can start working on the other changes I have requested on this PR.

raghumanimehta · 2026-03-01T03:59:51Z

src/local_pathfinding/local_pathfinding/coord_systems.py

    return [latlon_to_xy(reference=reference_latlon, latlon=pos) for pos in lat_lon_list]
+
+
+def rot_to_rad_per_sec(rot: int) -> float:


A lot of Magic number violations. You don't need to make all of them constants, but can you explain some of the calculations you are doing here? Wherever it makes sense to you, please repalce magic numbers with appropriately named constants.

raghumanimehta · 2026-03-01T04:00:13Z

src/local_pathfinding/local_pathfinding/obstacles.py

            self.ais_ship = ais_ship

-        projected_distance = self._calculate_projected_distance()
+        # Calculate ROT in radians per second


comment not needed

raghumanimehta · 2026-03-01T22:16:12Z

src/local_pathfinding/local_pathfinding/obstacles.py

+        cog_rad = math.radians(self.ais_ship.cog.heading)
+        x, y = cs.latlon_to_xy(self.reference, self.ais_ship.lat_lon)
+        projected_distance = self._calculate_projected_distance(cog_rad)
+        bow_y = self.length / 2


what does bow_y do?

raghumanimehta · 2026-03-01T22:16:39Z

src/local_pathfinding/local_pathfinding/obstacles.py

+
+        if projected_distance == PROJ_DISTANCE_NO_COLLISION:
+            # If no collision detected, create small collision zone around the boat
+            RADIUS_MULTIPLIER = 5


How did you come up with 5?

raghumanimehta · 2026-03-01T22:18:13Z

src/local_pathfinding/local_pathfinding/obstacles.py

+                radius, resolution=16
+            )
+            raw = self._translate_collision_zone(boat_collision_zone)
+            self._raw_collision_zone = raw


I would suggest assigning self._raw_collision_zone directly.

raghumanimehta · 2026-03-01T22:18:48Z

src/local_pathfinding/local_pathfinding/obstacles.py

-                [-collision_zone_width, self.length / 2 + projected_distance],
-                [collision_zone_width, self.length / 2 + projected_distance],
-                [self.width / 2, -self.length / 2],
+        elif abs(rot_rps) < 1e-4:


How did you come up with this threshold? + Magic number

raghumanimehta · 2026-03-01T22:19:04Z

src/local_pathfinding/local_pathfinding/obstacles.py

-                [self.width / 2, -self.length / 2],
+        elif abs(rot_rps) < 1e-4:
+            # Straight line
+            collision_zone_width = projected_distance * COLLISION_ZONE_STRETCH_FACTOR * self.width


This code is same as earlier, right?

raghumanimehta · 2026-03-01T22:25:14Z

src/local_pathfinding/local_pathfinding/coord_systems.py

+    if abs(rot) == 127:
+        rot_dpm = 10.0
+    else:
+        rot_dpm = (abs(rot) / 4.733) ** 2


Question: why not remove the abs? It seems that we are first computing the magnitude then multiplying the direction on line 264 (correct me if i am wrong). Doesn't it make sense to just do

Suggested change

rot_dpm = (abs(rot) / 4.733) ** 2

rot_dpm = (rot / 4.733) ** 2

This now encodes direction directly. I am not sure if this would produce the correct result, please verify this change through testing.

raghumanimehta · 2026-03-01T22:25:51Z

src/local_pathfinding/local_pathfinding/obstacles.py

+            prepared.prep(self.collision_zone)
+        else:
+            # Turning motion
+            R = speed_kmps / abs(rot_rps)


Can you add the units of R to the variable name please?

This is how much the boat translates per radian right?

raghumanimehta · 2026-03-01T22:29:18Z

src/local_pathfinding/local_pathfinding/obstacles.py

+            turn_angle = rot_rps * dt
+
+            # Center of rotation in world frame
+            cx = x - R * math.sin(cog_rad) * math.copysign(1, rot_rps)


I haven't verified this but, could you please check if cog is in the right coordinate system for our applicaiton? Remember, our coordinate systems were a mess and this file was written before we discovered the mess. Please take a look whenever you have time.

Please note the documentaiton might be wrong in places; i appologize for that. I am in the process of fixing it.

I will also point out that i had discovered that math.sin() and other trigonometric functions might need to be flipped to be used correctly. Please look into that as well.

Also, can you please explain the math here. Especially why does cx = x - R ... whereas cy = y + R...? That is, why is there a discrepency in the signs of those calculations. It is not immediately obvious to me.

rnar17 added 2 commits February 22, 2026 19:55

added tests

9412d5d

Added other changes

b38100d

rnar17 requested a review from raghumanimehta as a code owner February 23, 2026 04:10

raghumanimehta requested changes Mar 1, 2026

View reviewed changes

raghumanimehta requested a review from SPDonaghy March 1, 2026 22:35

raghumanimehta assigned AMaharaj16 Mar 1, 2026

raghumanimehta requested a review from AMaharaj16 March 1, 2026 22:53

raghumanimehta added the path Pathfinding team label Mar 2, 2026

raghumanimehta unassigned AMaharaj16 Mar 2, 2026

		return [latlon_to_xy(reference=reference_latlon, latlon=pos) for pos in lat_lon_list]


		def rot_to_rad_per_sec(rot: int) -> float:

	rot_dpm = (abs(rot) / 4.733) ** 2
	rot_dpm = (rot / 4.733) ** 2

Conversation

rnar17 commented Feb 23, 2026

Description

Verification

Uh oh!

raghumanimehta left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants