Fixing Follow Crop Navigation (#103)

* tests follow_crop_navigation * correcting tests * correcting tests * optimizing tests * adjusting minimum crop number and max distance for calculating row direction * merged with 'main' * deleting three time detection of crop check * optimizing drive distance * refining tests * updating tests * tests with correctly calculated values * cleanup --------- Co-authored-by: Pascal Schade <[email protected]>
zauberzeug · Jul 30, 2024 · 2449b30 · 2449b30
1 parent 87c3805
commit 2449b30
Show file tree

Hide file tree

Showing 4 changed files with 142 additions and 31 deletions.
diff --git a/backup/rb34/field_friend.automations.implements.weeding_screw.json b/backup/rb34/field_friend.automations.implements.weeding_screw.json
@@ -1,10 +1,10 @@
 {
-    "with_drilling": false,
-    "with_chopping": false,
-    "chop_if_no_crops": false,
-    "cultivated_crop": null,
-    "crop_safety_distance": 0.02,
-    "with_punch_check": false,
-    "weed_screw_depth": 0.14,
-    "max_crop_distance": 0.08
-}
+  "with_drilling": false,
+  "with_chopping": false,
+  "chop_if_no_crops": false,
+  "cultivated_crop": null,
+  "crop_safety_distance": 0.02,
+  "with_punch_check": false,
+  "weed_screw_depth": 0.14,
+  "max_crop_distance": 0.08
+}
diff --git a/backup/rb34/field_friend.automations.plant_locator.json b/backup/rb34/field_friend.automations.plant_locator.json
@@ -1,9 +1,7 @@
 {
-    "minimum_weed_confidence": 0.3,
-    "minimum_crop_confidence": 0.3,
-    "autoupload": "filtered",
-    "upload_images": false,
-    "tags": [
-        "kiebitz"
-    ]
-}
+  "minimum_weed_confidence": 0.3,
+  "minimum_crop_confidence": 0.3,
+  "autoupload": "filtered",
+  "upload_images": false,
+  "tags": ["kiebitz"]
+}
diff --git a/backup/rb34/field_friend.automations.plant_provider.json b/backup/rb34/field_friend.automations.plant_provider.json
@@ -1,8 +1,8 @@
 {
-    "match_distance": 0.1,
-    "crop_spacing": 0.18,
-    "predict_crop_position": false,
-    "prediction_confidence": 0.3,
-    "crop_confidence_threshold": 0.6,
-    "weed_confidence_threshold": 0.35
-}
+  "match_distance": 0.1,
+  "crop_spacing": 0.18,
+  "predict_crop_position": false,
+  "prediction_confidence": 0.3,
+  "crop_confidence_threshold": 0.6,
+  "weed_confidence_threshold": 0.35
+}
diff --git a/tests/test_navigation.py b/tests/test_navigation.py
@@ -86,20 +86,133 @@ async def test_driving_straight_line_with_slippage(system: System):
     assert system.odometer.prediction.point.y == pytest.approx(0.0, abs=0.1)
 
 
-async def test_follow_crops(system: System, detector: rosys.vision.DetectorSimulation):
-    for i in range(20):
+async def test_follow_crops_no_direction(system: System, detector: rosys.vision.DetectorSimulation):
+    for i in range(1, 3):
+        x = i*0.4
+        y = i/10
+        p = rosys.geometry.Point3d(x=x, y=y, z=0)
+        detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='maize', position=p))
+    system.current_navigation = system.follow_crops_navigation
+    assert isinstance(system.current_navigation.implement, Recorder)
+    system.automator.start()
+    await forward(2)
+    assert system.automator.is_running
+    await forward(until=lambda: not system.automator.is_running, timeout=300)
+    assert not system.automator.is_running, 'automation should stop if no crops are detected anymore'
+    assert system.odometer.prediction.distance(rosys.geometry.Point(x=0, y=0)) == pytest.approx(2 * 0.4 + 0.5, abs=0.1)
+    assert system.odometer.prediction.point.x == pytest.approx(1.3, abs=0.1)
+    assert system.odometer.prediction.point.y == pytest.approx(0, abs=0.01)
+    assert system.odometer.prediction.yaw_deg == pytest.approx(0, abs=1.0)
+
+
+async def test_follow_crops_empty(system: System, detector: rosys.vision.DetectorSimulation):
+    system.current_navigation = system.follow_crops_navigation
+    assert isinstance(system.current_navigation.implement, Recorder)
+    system.automator.start()
+    await forward(2)
+    assert system.automator.is_running
+    await forward(until=lambda: not system.automator.is_running, timeout=300)
+    assert not system.automator.is_running, 'automation should stop if no crops are detected anymore'
+    assert system.odometer.prediction.point.x == pytest.approx(0.5, abs=0.1)
+    assert system.odometer.prediction.point.y == pytest.approx(0, abs=0.01)
+    assert system.odometer.prediction.yaw_deg == pytest.approx(0, abs=1.0)
+
+
+async def test_follow_crops_straight(system: System, detector: rosys.vision.DetectorSimulation):
+    for i in range(10):
+        x = i/10
+        p = rosys.geometry.Point3d(x=x, y=0, z=0)
+        detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='maize', position=p))
+    system.current_navigation = system.follow_crops_navigation
+    assert isinstance(system.current_navigation.implement, Recorder)
+    system.automator.start()
+    await forward(2)
+    assert system.automator.is_running
+    await forward(until=lambda: not system.automator.is_running, timeout=300)
+    assert not system.automator.is_running, 'automation should stop if no crops are detected anymore'
+    assert system.odometer.prediction.point.x == pytest.approx(1.5, abs=0.1)
+    assert system.odometer.prediction.point.y == pytest.approx(0, abs=0.1)
+    assert system.odometer.prediction.yaw_deg == pytest.approx(0, abs=1.0)
+
+
+async def test_follow_crops_adjust(system: System, detector: rosys.vision.DetectorSimulation):
+    for i in range(1, 51):
+        x = i*0.4
+        y = -(i/20)
+        p = rosys.geometry.Point3d(x=x, y=y, z=0)
+        detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='maize', position=p))
+    system.current_navigation = system.follow_crops_navigation
+    assert isinstance(system.current_navigation.implement, Recorder)
+    system.automator.start()
+    await forward(2)
+    assert system.automator.is_running
+    await forward(until=lambda: not system.automator.is_running, timeout=300)
+    assert not system.automator.is_running, 'automation should stop if no crops are detected anymore'
+    assert system.odometer.prediction.point.x == pytest.approx(20.5, abs=0.1)
+    assert system.odometer.prediction.point.y == pytest.approx(-2.5, abs=0.1)
+    assert system.odometer.prediction.yaw_deg == pytest.approx(-7.125, abs=1.0)
+
+
+async def test_follow_crops_curve(system: System, detector: rosys.vision.DetectorSimulation):
+    for i in range(1, 56):
         x = i/10.0
-        p = rosys.geometry.Point3d(x=x, y=(x/2) ** 3, z=0)
+        p = rosys.geometry.Point3d(x=x, y=(x/4) ** 2, z=0)
         p = system.odometer.prediction.transform3d(p)
         detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='maize', position=p))
     system.current_navigation = system.follow_crops_navigation
-    assert isinstance(system.current_navigation.implement, Recorder)
     system.automator.start()
+    assert isinstance(system.current_navigation.implement, Recorder)
     await forward(until=lambda: system.automator.is_running)
     await forward(until=lambda: system.automator.is_stopped)
-    assert system.odometer.prediction.point.x == pytest.approx(2.2, abs=0.1)
-    assert system.odometer.prediction.point.y == pytest.approx(0.45, abs=0.1)
-    assert system.odometer.prediction.yaw_deg == pytest.approx(40.0, abs=5.0)
+    assert not system.automator.is_running, 'automation should stop if no crops are detected anymore'
+    assert system.odometer.prediction.point.x == pytest.approx(5.9, abs=0.1)
+    assert system.odometer.prediction.point.y == pytest.approx(2.16, abs=0.1)
+    assert system.odometer.prediction.yaw_deg == pytest.approx(33.69, abs=3.0)
+
+
+async def test_follow_crops_outlier(system: System, detector: rosys.vision.DetectorSimulation):
+    for i in range(10):
+        x = i/10
+        y = 0
+        p = rosys.geometry.Point3d(x=x, y=y, z=0)
+        detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='maize', position=p))
+    outlier = rosys.geometry.Point3d(x=1.1, y=0.2, z=0)
+    detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='maize', position=outlier))
+    for i in range(10):
+        x = i/10 + 1.1
+        y = 0
+        p = rosys.geometry.Point3d(x=x, y=y, z=0)
+        detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='maize', position=p))
+    system.current_navigation = system.follow_crops_navigation
+    assert isinstance(system.current_navigation.implement, Recorder)
+    system.automator.start()
+    await forward(2)
+    assert system.automator.is_running
+    await forward(until=lambda: not system.automator.is_running, timeout=300)
+    assert not system.automator.is_running, 'automation should stop if no crops are detected anymore'
+    assert system.odometer.prediction.point.x == pytest.approx(2.6, abs=0.1)
+    assert system.odometer.prediction.point.y == pytest.approx(0, abs=0.05)
+    assert system.odometer.prediction.yaw_deg == pytest.approx(0, abs=1)
+
+
+async def test_follow_crops_outlier_last(system: System, detector: rosys.vision.DetectorSimulation):
+    for i in range(20):
+        x = i/10
+        y = 0
+        p = rosys.geometry.Point3d(x=x, y=y, z=0)
+        detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='maize', position=p))
+    outlier = rosys.geometry.Point3d(x=2.1, y=-0.2, z=0)
+    detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='maize', position=outlier))
+    system.current_navigation = system.follow_crops_navigation
+    assert isinstance(system.current_navigation.implement, Recorder)
+    system.automator.start()
+    await forward(2)
+    assert system.automator.is_running
+    await forward(until=lambda: not system.automator.is_running, timeout=300)
+    assert not system.automator.is_running, 'automation should stop if no crops are detected anymore'
+    assert system.odometer.prediction.point.x == pytest.approx(2.6, abs=0.1)
+    assert 0 >= system.odometer.prediction.point.y >= -0.25
+    assert 0 >= system.odometer.prediction.yaw_deg >= -45
 
 
 async def test_follow_crops_with_slippage(system: System, detector: rosys.vision.DetectorSimulation):