From 2f66d574d2abe615c3ef59c1c774bdb0a32a1de7 Mon Sep 17 00:00:00 2001
From: Pascal Schade <165774906+pascalzauberzeug@users.noreply.github.com>
Date: Mon, 9 Sep 2024 16:39:09 +0200
Subject: [PATCH] Fix implement projection (#163)

* fix implement projection

* cleanup

* use new Pose3d -> Point3d property

* cleanup
---
 .../interface/components/camera_card.py        | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/field_friend/interface/components/camera_card.py b/field_friend/interface/components/camera_card.py
index d593a962..2ecb2067 100644
--- a/field_friend/interface/components/camera_card.py
+++ b/field_friend/interface/components/camera_card.py
@@ -11,7 +11,7 @@
 from field_friend.automations.implements.weeding_implement import WeedingImplement
 
 from ...automations import PlantLocator, Puncher
-from ...hardware import Axis, FieldFriend, FlashlightPWM, FlashlightPWMV2, Tornado
+from ...hardware import Axis, FlashlightPWM, FlashlightPWMV2, Tornado
 from .calibration_dialog import calibration_dialog
 
 if TYPE_CHECKING:
@@ -126,8 +126,7 @@ def update_content(self) -> None:
         if image and image.detections:
             svg += self.to_svg(image.detections)
         svg += self.build_svg_for_plant_provider()
-        # TODO: fix in later PR
-        # svg += self.build_svg_for_implement()
+        svg += self.build_svg_for_implement()
         self.image_view.set_content(svg)
 
     def on_mouse_move(self, e: MouseEventArguments):
@@ -196,15 +195,16 @@ def to_svg(self, detections: rosys.vision.Detections) -> str:
     def build_svg_for_implement(self) -> str:
         if not isinstance(self.system.current_implement, WeedingImplement) or self.camera is None or self.camera.calibration is None:
             return ''
-        tool_3d = self.odometer.prediction.point_3d() + \
-            rosys.geometry.Point3d(x=self.field_friend.WORK_X, y=self.field_friend.y_axis.position, z=0)
+        tool_3d = rosys.geometry.Pose3d(x=self.field_friend.WORK_X, y=self.field_friend.y_axis.position, z=0).in_frame(
+            self.odometer.prediction_frame).resolve().point_3d
         tool_2d = self.camera.calibration.project_to_image(tool_3d) / self.shrink_factor
         svg = f'<circle cx="{int(tool_2d.x)}" cy="{int(tool_2d.y)}" r="10" fill="black"/>'
-        min_tool_3d = self.odometer.prediction.point_3d() + \
-            rosys.geometry.Point3d(x=self.field_friend.WORK_X, y=self.field_friend.y_axis.min_position, z=0)
+
+        min_tool_3d = rosys.geometry.Pose3d(x=self.field_friend.WORK_X, y=self.field_friend.y_axis.min_position, z=0).in_frame(
+            self.odometer.prediction_frame).resolve().point_3d
         min_tool_2d = self.camera.calibration.project_to_image(min_tool_3d) / self.shrink_factor
-        max_tool_3d = self.odometer.prediction.point_3d() + \
-            rosys.geometry.Point3d(x=self.field_friend.WORK_X, y=self.field_friend.y_axis.max_position, z=0)
+        max_tool_3d = rosys.geometry.Pose3d(x=self.field_friend.WORK_X, y=self.field_friend.y_axis.max_position, z=0).in_frame(
+            self.odometer.prediction_frame).resolve().point_3d
         max_tool_2d = self.camera.calibration.project_to_image(max_tool_3d) / self.shrink_factor
         svg += f'<line x1="{int(min_tool_2d.x)}" y1="{int(min_tool_2d.y)}" x2="{int(max_tool_2d.x)}" y2="{int(max_tool_2d.y)}" stroke="black" stroke-width="2" />'
         if self.show_weeds_to_handle: