Done

src/arm_harverster/CMakeLists.txt

@@ -8,6 +8,7 @@ install(PROGRAMS
   scripts/move_to_point.py
   scripts/arm_point_controller.py
   scripts/mission.py
+  scripts/detection.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

src/arm_harverster/scripts/arm_point_controller.py

@@ -7,8 +7,9 @@ import time
 
 # ROS 2 Message, Action, and Service Imports
 from moveit_msgs.action import MoveGroup, ExecuteTrajectory
-from moveit_msgs.msg import Constraints, RobotState
+from moveit_msgs.msg import Constraints, RobotState, PositionConstraint, OrientationConstraint, BoundingVolume
 from moveit_msgs.srv import GetCartesianPath, GetPositionFK
+from shape_msgs.msg import SolidPrimitive
 from geometry_msgs.msg import PoseStamped, Pose
 from sensor_msgs.msg import JointState
 
@@ -36,7 +37,7 @@ class CameraArmController(Node):
         self.get_logger().info("Connecting to Forward Kinematics service...")
         self._fk_client.wait_for_service()
         
-        # 5. Subscriber to listen to current joint states (Required to compute current position)
+        # 5. Subscriber to listen to current joint states
         self.current_joint_state = None
         self._joint_sub = self.create_subscription(
             JointState,
@@ -52,13 +53,24 @@ class CameraArmController(Node):
         self.current_joint_state = msg
 
     def get_horizontal_camera_quaternion(self):
-        """Calculates a 90-degree pitch offset to keep the camera level with the horizon"""
-        pitch_angle = math.radians(-90.0)
+        """Keeps camera level with horizon but yaws it 90 degrees sideways"""
+        pitch = math.radians(-90.0) # Keep horizon level
+        yaw = math.radians(90.0)    # Rotate 90 degrees sideways (use -90.0 for the opposite side)
+        roll = 0.0
+
+        # Standard Euler to Quaternion conversion equations
+        cy = math.cos(yaw * 0.5)
+        sy = math.sin(yaw * 0.5)
+        cp = math.cos(pitch * 0.5)
+        sp = math.sin(pitch * 0.5)
+        cr = math.cos(roll * 0.5)
+        sr = math.sin(roll * 0.5)
+
         q = Pose().orientation
-        q.x = 0.0
-        q.y = math.sin(pitch_angle / 2.0)
-        q.z = 0.0
-        q.w = math.cos(pitch_angle / 2.0)
+        q.w = cr * cp * cy + sr * sp * sy
+        q.x = sr * cp * cy - cr * sp * sy
+        q.y = cr * sp * cy + sr * cp * sy
+        q.z = cr * cp * sy - sr * sp * cy
         return q
 
     def get_current_end_effector_pose(self):
@@ -101,31 +113,21 @@ class CameraArmController(Node):
         target_pose.pose.position.x = float(x)
         target_pose.pose.position.y = float(y)
         target_pose.pose.position.z = float(z)
-        
-        # Enforces the horizontal alignment strictly at the final destination pose
         target_pose.pose.orientation = self.get_horizontal_camera_quaternion()
 
         # Add target pose to the path request constraints
         goal_constraints = Constraints()
         goal_msg.request.goal_constraints.append(goal_constraints)
-        
-        # MoveIt handles position and end-point orientation without path constraints
-        # By avoiding constraints during movement, the planner should avoid RRTConnect failures.
         goal_msg.request.workspace_parameters.header.frame_id = "base_link"
         
-        # Pack the target pose directly into the goal message requests
-        from moveit_msgs.msg import PositionConstraint, OrientationConstraint
-        
-        # Create a simple end-state constraint matching our target pose
         # Position Constraint
         pos_con = PositionConstraint()
         pos_con.header.frame_id = "base_link"
         pos_con.link_name = "tool_link"
-        from moveit_msgs.msg import BoundingVolume
-        from shape_msgs.msg import SolidPrimitive
+        
         box = SolidPrimitive()
         box.type = SolidPrimitive.BOX
-        box.dimensions = [0.01, 0.01, 0.01] # Tight convergence tolerance
+        box.dimensions = [0.01, 0.01, 0.01] 
         volume = BoundingVolume()
         volume.primitives.append(box)
         
@@ -141,25 +143,40 @@ class CameraArmController(Node):
         ori_con.header.frame_id = "base_link"
         ori_con.link_name = "tool_link"
         ori_con.orientation = target_pose.pose.orientation
-        ori_con.absolute_x_axis_tolerance = 0.05
-        ori_con.absolute_y_axis_tolerance = 0.05
-        ori_con.absolute_z_axis_tolerance = 0.05
+        ori_con.absolute_x_axis_tolerance = 0.005
+        ori_con.absolute_y_axis_tolerance = 0.005
+        ori_con.absolute_z_axis_tolerance = 0.005
         ori_con.weight = 1.0
         
         goal_constraints.position_constraints.append(pos_con)
         goal_constraints.orientation_constraints.append(ori_con)
 
         send_goal_future = self._action_client.send_goal_async(goal_msg)
-        rclpy.spin_until_future_complete(self, send_goal_future)
+
+        # 2. CREATE A TEMPORARY EXECUTOR FOR THIS BLOCKED MOVE
+        from rclpy.executors import SingleThreadedExecutor
+        temp_executor = SingleThreadedExecutor()
+        temp_executor.add_node(self) # Temporarily borrow this node context
+
+        # 3. Spin our isolated executor until MoveIt decides to accept or reject the goal
+        temp_executor.spin_until_future_complete(send_goal_future)
         
         goal_handle = send_goal_future.result()
         if not goal_handle.accepted:
             self.get_logger().error("Point-to-Point goal rejected by MoveIt!")
+            temp_executor.remove_node(self) # Clean up before returning
             return False
 
         self.get_logger().info("Goal accepted! Executing path...")
+        
+        # 4. Request the execution outcome and spin our local executor again until the arm physically lands
         get_result_future = goal_handle.get_result_async()
-        rclpy.spin_until_future_complete(self, get_result_future)
+        temp_executor.spin_until_future_complete(get_result_future)
+        
+        # 5. MANDATORY CLEANUP: Release the node back to your main script's executor loop
+        temp_executor.remove_node(self)
+        
+        self.get_logger().info("Ruch ramienia zakończony sukcesem!")
         return True
 
     def execute_straight_camera_sweep(self, distance=0.2):
@@ -216,19 +233,52 @@ class CameraArmController(Node):
             self.get_logger().error("Trajectory execution rejected.")
             return False
 
+    def home_arm(self):
+        self.send_pt_to_pt_goal(0.35, 0.1, 1.35)
+
+
+    def harvest(self):
+        inspection_x = 0.35  # 45cm forward past the shoulder joint
+        inspection_y = -0.4  # Centered
+        inspection_z = 1.35 
+        at_target = self.send_pt_to_pt_goal(inspection_x, inspection_y, inspection_z)
+
+
+
 def main(args=None):
     rclpy.init(args=args)
     node = CameraArmController()
 
-    # Coordinates are now evaluated relative to the arm's base link.
-    # Reaches 40cm forward, 30cm down toward crops. It will rotate freely during
-    # transit and level out perfectly flat once it locks into the target destination.
-    success = node.send_pt_to_pt_goal(.2, 0.3, 0.3)
+    # --- DEFINE ROUTINE CONFIGURATIONS ---
+    # Targets are relative to the arm's base link origin (inverted setup)
+    inspection_x = 0.35  # 45cm forward past the shoulder joint
+    inspection_y = -0.4  # Centered
+    inspection_z = 1.35  # 35cm down toward the field plants
     
-    if success:
-        time.sleep(2.0) 
-        # Execute the forward crawl sweep
-        node.execute_straight_camera_sweep(distance=0.2)
+    home_x = 0.35        # Tucked back close to the vehicle frame
+    home_y = 0.10
+    home_z = 1.35        # Retracted up high out of the crop height line
+
+    # 1. Move to Predefined Inspection Target Point
+    node.get_logger().info("Starting inspection routine. Advancing to inspection coordinate...")
+    at_target = node.send_pt_to_pt_goal(inspection_x, inspection_y, inspection_z)
+    
+    if at_target:
+        # 2. Arrived at target: Wait for sensors to settle/take pictures
+        wait_seconds = 3.0
+        node.get_logger().info(f"Arrived at inspection point. Holding position for {wait_seconds} seconds for camera sweep...")
+        time.sleep(wait_seconds) 
+        
+        # 3. Return to base point (Safe Home Configuration)
+        node.get_logger().info("Inspection complete. Returning to safe home position...")
+        returned_home = node.send_pt_to_pt_goal(home_x, home_y, home_z)
+        
+        if returned_home:
+            node.get_logger().info("Arm securely home. Routine complete!")
+        else:
+            node.get_logger().error("Failed to return to home base point!")
+    else:
+        node.get_logger().error("Failed to reach initial inspection target!")
 
     node.destroy_node()
     rclpy.shutdown()

src/arm_harverster/scripts/detection.py

@@ -0,0 +1,118 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+# IMPORTUJEMY PROFIL QoS DLA SENSORÓW
+from rclpy.qos import qos_profile_sensor_data 
+import numpy as np
+import cv2
+from ultralytics import YOLO
+
+class YoloGazeboDetector(Node):
+    def __init__(self):
+        super().__init__('yolo_gazebo_detector')
+        
+        self.get_logger().info("Ładowanie modelu YOLO...")
+        self.model = YOLO('best.pt') 
+        self.model.to('cpu') 
+        
+        self.window_name = "YOLOv8 - Gazebo Camera"
+        cv2.namedWindow(self.window_name, cv2.WINDOW_NORMAL)
+        cv2.resizeWindow(self.window_name, 640, 480)
+        
+        self.frame_count = 0
+        self.camera_topic = '/arm_camera/image_raw' 
+        
+        # ZMIANA: Zastępujemy liczbę queue_size gotowym profilem qos_profile_sensor_data
+        self.subscription = self.create_subscription(
+            Image,
+            self.camera_topic,
+            self.camera_callback,
+            qos_profile_sensor_data
+        )
+        
+        self.window_timer = self.create_timer(0.03, self.refresh_blank_window)
+        self.latest_annotated_frame = None
+        
+        self.get_logger().info(f"Node i Okno GUI gotowe (Zaktualizowano QoS). Słucham na: {self.camera_topic}")
+
+    def refresh_blank_window(self):
+        if self.latest_annotated_frame is None:
+            loading_frame = np.zeros((480, 640, 3), dtype=np.uint8)
+            cv2.putText(loading_frame, "Oczekiwanie na obrazy z Gazebo...", (80, 240),
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
+            cv2.imshow(self.window_name, loading_frame)
+        else:
+            cv2.imshow(self.window_name, self.latest_annotated_frame)
+        cv2.waitKey(1)
+
+    def camera_callback(self, msg):
+        self.frame_count += 1
+        
+        # Wymuszony log dla każdej klatki, żeby potwierdzić wejście do funkcji
+        self.get_logger().info(f"--- ODEBRANO WIADOMOŚĆ (Klatka {self.frame_count}) ---")
+        self.get_logger().info(f"Wymiary z nagłówka: Width={msg.width}, Height={msg.height}, Encoding={msg.encoding}")
+        self.get_logger().info(f"Rozmiar surowej tablicy danych (msg.data): {len(msg.data)} bajtów")
+
+        if len(msg.data) == 0:
+            self.get_logger().error("⚠️ Krytyczny błąd: Tablica msg.data jest całkowicie pusta!")
+            return
+
+        try:
+            # Konwersja do formatu numpy
+            flat_img_array = np.frombuffer(msg.data, dtype=np.uint8)
+            
+            # Dynamiczne obliczanie oczekiwanego rozmiaru na podstawie kanałów
+            channels = 3
+            if 'rgba' in msg.encoding or 'bgra' in msg.encoding:
+                channels = 4
+            elif 'mono' in msg.encoding:
+                channels = 1
+                
+            expected_elements = msg.height * msg.width * channels
+            if flat_img_array.size != expected_elements:
+                self.get_logger().error(f"⚠️ Niezgodność rozmiarów! Otrzymano {flat_img_array.size} elementów, a oczekiwano {expected_elements}")
+                # Próba awaryjnego dopasowania
+                cv_image = flat_img_array.reshape((msg.height, msg.step // channels, channels))[:, :msg.width, :]
+            else:
+                cv_image = flat_img_array.reshape((msg.height, msg.width, channels))
+            
+            # Obsługa konwersji przestrzeni barw
+            if 'rgb' in msg.encoding.lower():
+                cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR)
+            elif channels == 4:
+                cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGRA2BGR)
+                
+        except Exception as e:
+            self.get_logger().error(f"💥 Błąd krytyczny podczas przetwarzania macierzy: {e}")
+            return
+
+        # Jeśli doszliśmy tutaj, obraz jest poprawny. Przekazujemy do YOLO
+        try:
+            results = self.model(cv_image, verbose=False, imgsz=320) 
+            annotated_frame = cv_image.copy()
+            for r in results:
+                annotated_frame = r.plot()
+                
+            # Sukces! Podmieniamy klatkę dla okna wyświetlacza
+            self.latest_annotated_frame = annotated_frame
+        except Exception as e:
+            self.get_logger().error(f"Błąd podczas wnioskowania YOLO: {e}")
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = YoloGazeboDetector()
+    
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.get_logger().info("Zamykanie node'a...")
+        cv2.destroyAllWindows()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/arm_harverster/scripts/mission.py

@@ -9,6 +9,7 @@ import math
 import time
 import sys
 import numpy as np
+from arm_point_controller import CameraArmController
 
 try:
     import pygame
@@ -34,6 +35,9 @@ class HarvesterStateMachine(Node):
         self.lidar_left_sub = self.create_subscription(LaserScan, '/lidar_left', self.lidar_left_callback, qos_profile)
         self.lidar_right_sub = self.create_subscription(LaserScan, '/lidar_right', self.lidar_right_callback, qos_profile)
 
+        self.arm = CameraArmController()
+        self.arm.home_arm()
+
         self.STATE_INITIALIZING = "INITIALIZING"
         self.STATE_GPS_DRIVE    = "GPS_DRIVE"
         self.STATE_LIDAR_DRIVE  = "LIDAR_DRIVE"
@@ -261,7 +265,13 @@ class HarvesterStateMachine(Node):
 
     def execute_harvesting(self):
         self.stop_robot()
-        time.sleep(2.0)
+
+        time.sleep(0.5)
+        self.arm.harvest()
+        time.sleep(0.5)
+        self.arm.home_arm()
+        time.sleep(0.5)
+
         self.harvesting_done_in_this_row = True
         self.current_state = self.STATE_LIDAR_DRIVE
 

src/robotarm_description/urdf/robot_arm.urdf

@@ -66,7 +66,7 @@
     <child link="link_2"/>
     <origin rpy="0 0 0" xyz="0 0 0.48"/>
     <axis xyz="0 1 0"/>
-    <limit effort="300.0" lower="-2.0" upper="2.0" velocity="2.0"/>
+    <limit effort="300.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="2.0"/>
     <dynamics damping="1.0" friction="1.0"/>
   </joint>
   <link name="link_2">
@@ -94,7 +94,7 @@
     <child link="link_3"/>
     <origin rpy="0 0 0" xyz="0 0 0.6400000000000001"/>
     <axis xyz="0 1 0"/>
-    <limit effort="200.0" lower="-2.5" upper="2.5" velocity="2.5"/>
+    <limit effort="200.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="2.5"/>
     <dynamics damping="1.0" friction="1.0"/>
   </joint>
   <link name="link_3">
@@ -150,7 +150,7 @@
     <child link="link_5"/>
     <origin rpy="0 0 0" xyz="0 0 0.32000000000000006"/>
     <axis xyz="0 1 0"/>
-    <limit effort="100.0" lower="-2.0" upper="2.0" velocity="3.0"/>
+    <limit effort="100.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.0"/>
     <dynamics damping="0.5" friction="0.5"/>
   </joint>
   <link name="link_5">

src/robotarm_description/urdf/robot_arm.urdf.xacro

@@ -64,7 +64,7 @@
     <child link="link_2"/>
     <origin xyz="0 0 ${0.3 * LENGTH_SCALE}" rpy="0 0 0"/>
     <axis xyz="0 1 0"/>
-    <limit lower="-2.0" upper="2.0" effort="300.0" velocity="2.0"/>
+    <limit lower="-${PI}" upper="${PI}" effort="300.0" velocity="2.0"/>
     <dynamics damping="1.0" friction="1.0"/>
   </joint>
 
@@ -88,7 +88,7 @@
     <child link="link_3"/>
     <origin xyz="0 0 ${0.4 * LENGTH_SCALE}" rpy="0 0 0"/>
     <axis xyz="0 1 0"/>
-    <limit lower="-2.5" upper="2.5" effort="200.0" velocity="2.5"/>
+    <limit lower="-${PI}" upper="${PI}" effort="200.0" velocity="2.5"/>
     <dynamics damping="1.0" friction="1.0"/>
   </joint>
 
@@ -136,7 +136,7 @@
     <child link="link_5"/>
     <origin xyz="0 0 ${0.2 * LENGTH_SCALE}" rpy="0 0 0"/>
     <axis xyz="0 1 0"/>
-    <limit lower="-2.0" upper="2.0" effort="100.0" velocity="3.0"/>
+    <limit lower="-${PI}" upper="${PI}" effort="100.0" velocity="3.0"/>
     <dynamics damping="0.5" friction="0.5"/>
   </joint>