diff --git a/src/arm_harverster/CMakeLists.txt b/src/arm_harverster/CMakeLists.txt index 6330ee8..f641604 100644 --- a/src/arm_harverster/CMakeLists.txt +++ b/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} ) diff --git a/src/arm_harverster/scripts/arm_point_controller.py b/src/arm_harverster/scripts/arm_point_controller.py index abea795..72aff6f 100755 --- a/src/arm_harverster/scripts/arm_point_controller.py +++ b/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() diff --git a/src/arm_harverster/scripts/best.pt b/src/arm_harverster/scripts/best.pt new file mode 100644 index 0000000..31e9bff Binary files /dev/null and b/src/arm_harverster/scripts/best.pt differ diff --git a/src/arm_harverster/scripts/detection.py b/src/arm_harverster/scripts/detection.py new file mode 100755 index 0000000..fda1beb --- /dev/null +++ b/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() \ No newline at end of file diff --git a/src/arm_harverster/scripts/mission.py b/src/arm_harverster/scripts/mission.py index e274172..f0c297d 100755 --- a/src/arm_harverster/scripts/mission.py +++ b/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 diff --git a/src/arm_moveit_config/launch/sim.launch.py b/src/arm_moveit_config/launch/sim.launch.py old mode 100644 new mode 100755 diff --git a/src/robotarm_description/urdf/robot_arm.urdf b/src/robotarm_description/urdf/robot_arm.urdf index e5cb8b0..9151505 100644 --- a/src/robotarm_description/urdf/robot_arm.urdf +++ b/src/robotarm_description/urdf/robot_arm.urdf @@ -66,7 +66,7 @@ - + @@ -94,7 +94,7 @@ - + @@ -150,7 +150,7 @@ - + diff --git a/src/robotarm_description/urdf/robot_arm.urdf.xacro b/src/robotarm_description/urdf/robot_arm.urdf.xacro index 48e9e61..844221d 100644 --- a/src/robotarm_description/urdf/robot_arm.urdf.xacro +++ b/src/robotarm_description/urdf/robot_arm.urdf.xacro @@ -64,7 +64,7 @@ - + @@ -88,7 +88,7 @@ - + @@ -136,7 +136,7 @@ - +