fix implement projection

zauberzeug · Aug 28, 2024 · d39ae41 · d39ae41
1 parent 4a6bd9d
commit d39ae41
Showing 1 changed file with 8 additions and 7 deletions.
diff --git a/field_friend/interface/components/camera_card.py b/field_friend/interface/components/camera_card.py
@@ -127,7 +127,7 @@ def update_content(self) -> None:
             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 +196,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.Point3d.from_tuple(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().translation)
         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.Point3d.from_tuple(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().translation)
         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.Point3d.from_tuple(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().translation)
         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: