From 1b98bea2691122ecfe58e20c790dc85db0dcedb1 Mon Sep 17 00:00:00 2001 From: Joseph Handsome Date: Mon, 23 Jun 2025 00:51:32 -0400 Subject: [PATCH 1/8] small node that echo's goal poses in an easy to visualize way no launch file, see setup.py for how to run --- .../goal_to_viz/goal_to_viz/__init__.py | 0 .../goal_to_viz/goal_to_viz/goal_to_viz.py | 50 +++++++++++++++++++ .../testing/goal_to_viz/package.xml | 18 +++++++ .../testing/goal_to_viz/resource/goal_to_viz | 0 src/subjugator/testing/goal_to_viz/setup.cfg | 4 ++ src/subjugator/testing/goal_to_viz/setup.py | 26 ++++++++++ 6 files changed, 98 insertions(+) create mode 100644 src/subjugator/testing/goal_to_viz/goal_to_viz/__init__.py create mode 100644 src/subjugator/testing/goal_to_viz/goal_to_viz/goal_to_viz.py create mode 100644 src/subjugator/testing/goal_to_viz/package.xml create mode 100644 src/subjugator/testing/goal_to_viz/resource/goal_to_viz create mode 100644 src/subjugator/testing/goal_to_viz/setup.cfg create mode 100644 src/subjugator/testing/goal_to_viz/setup.py diff --git a/src/subjugator/testing/goal_to_viz/goal_to_viz/__init__.py b/src/subjugator/testing/goal_to_viz/goal_to_viz/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/subjugator/testing/goal_to_viz/goal_to_viz/goal_to_viz.py b/src/subjugator/testing/goal_to_viz/goal_to_viz/goal_to_viz.py new file mode 100644 index 00000000..c9f55f89 --- /dev/null +++ b/src/subjugator/testing/goal_to_viz/goal_to_viz/goal_to_viz.py @@ -0,0 +1,50 @@ +from visualization_msgs.msg import Marker +from geometry_msgs.msg import Pose +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Quaternion + +class ConverterNode(Node): + def __init__(self): + super().__init__("converter_node") + self.publisher = self.create_publisher(Marker, '/visualization_marker', 10) + self.sub = self.create_subscription(Pose, "/goal/trajectory", self.goal_cb, 10) + self.latest:Pose = Pose() + + def goal_cb(self, msg:Pose): + self.latest = msg + + marker = Marker() + marker.header.frame_id = "odom" + marker.header.stamp = self.get_clock().now().to_msg() + marker.ns = "orientation" + marker.id = 0 + marker.type = Marker.ARROW + marker.action = Marker.ADD + + marker.pose.position.x = msg.position.x + marker.pose.position.y = msg.position.y + marker.pose.position.z = msg.position.z + + # Orientation: rotate 90 degrees around Z (yaw = π/2) + marker.pose.orientation = Quaternion(x=msg.orientation.x, y=msg.orientation.y, z=msg.orientation.z, w=msg.orientation.w) + + marker.scale.x = 1.0 # Arrow length + marker.scale.y = 0.1 # Arrow width + marker.scale.z = 0.1 # Arrow height + + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + marker.color.a = 1.0 + + self.publisher.publish(marker) + +def main(): + rclpy.init() + my_node = ConverterNode() + rclpy.spin(my_node) + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/src/subjugator/testing/goal_to_viz/package.xml b/src/subjugator/testing/goal_to_viz/package.xml new file mode 100644 index 00000000..d1168e48 --- /dev/null +++ b/src/subjugator/testing/goal_to_viz/package.xml @@ -0,0 +1,18 @@ + + + + goal_to_viz + 0.0.0 + TODO: Package description + jh + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/subjugator/testing/goal_to_viz/resource/goal_to_viz b/src/subjugator/testing/goal_to_viz/resource/goal_to_viz new file mode 100644 index 00000000..e69de29b diff --git a/src/subjugator/testing/goal_to_viz/setup.cfg b/src/subjugator/testing/goal_to_viz/setup.cfg new file mode 100644 index 00000000..2f8463e9 --- /dev/null +++ b/src/subjugator/testing/goal_to_viz/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/goal_to_viz +[install] +install_scripts=$base/lib/goal_to_viz diff --git a/src/subjugator/testing/goal_to_viz/setup.py b/src/subjugator/testing/goal_to_viz/setup.py new file mode 100644 index 00000000..bcacd2bd --- /dev/null +++ b/src/subjugator/testing/goal_to_viz/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'goal_to_viz' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='jh', + maintainer_email='nottellingu@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + "goal_to_viz = goal_to_viz.goal_to_viz:main" + ], + }, +) From 174cf4f0edce4c198f0d60856bfb01c81c405233 Mon Sep 17 00:00:00 2001 From: Joseph Handsome Date: Mon, 23 Jun 2025 00:51:44 -0400 Subject: [PATCH 2/8] adds viz node to launch file --- .../subjugator_bringup/launch/subjugator_setup.launch.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/subjugator/subjugator_bringup/launch/subjugator_setup.launch.py b/src/subjugator/subjugator_bringup/launch/subjugator_setup.launch.py index 2ad62178..f93cd9bb 100644 --- a/src/subjugator/subjugator_bringup/launch/subjugator_setup.launch.py +++ b/src/subjugator/subjugator_bringup/launch/subjugator_setup.launch.py @@ -117,6 +117,14 @@ def generate_launch_description(): output="both", ) + joe_goal_to_rviz = Node( + package="goal_to_viz", + executable="goal_to_viz", + name="goal_to_viz", + output="both", + ) + + return LaunchDescription( [ gui_cmd, @@ -128,5 +136,6 @@ def generate_launch_description(): controller, path_planner, trajectory_planner, + joe_goal_to_rviz, ], ) From 2cd444ae43ce92fb05ac40a1e600056cdf3ab316 Mon Sep 17 00:00:00 2001 From: Joseph Handsome Date: Fri, 11 Jul 2025 00:29:14 -0400 Subject: [PATCH 3/8] adds new action for tracking objects --- src/subjugator/subjugator_msgs/CMakeLists.txt | 1 + .../subjugator_msgs/action/TrackObject.action | 11 +++++++++++ 2 files changed, 12 insertions(+) create mode 100644 src/subjugator/subjugator_msgs/action/TrackObject.action diff --git a/src/subjugator/subjugator_msgs/CMakeLists.txt b/src/subjugator/subjugator_msgs/CMakeLists.txt index 47b5fc06..4fe432ba 100644 --- a/src/subjugator/subjugator_msgs/CMakeLists.txt +++ b/src/subjugator/subjugator_msgs/CMakeLists.txt @@ -38,6 +38,7 @@ rosidl_generate_interfaces( "action/NavigateThrough.action" "action/Movement.action" "action/Wait.action" + "action/TrackObject.action" DEPENDENCIES geometry_msgs std_msgs diff --git a/src/subjugator/subjugator_msgs/action/TrackObject.action b/src/subjugator/subjugator_msgs/action/TrackObject.action new file mode 100644 index 00000000..6ea7c0c0 --- /dev/null +++ b/src/subjugator/subjugator_msgs/action/TrackObject.action @@ -0,0 +1,11 @@ +# Goal +string object_type +string type_of_movement +# KeyValue[] parameters +--- +# Result +bool success +string message +--- +# Feedback +string status_message From 17083be32620551c03a48437101faad371db021a Mon Sep 17 00:00:00 2001 From: Joseph Handsome Date: Fri, 11 Jul 2025 00:29:28 -0400 Subject: [PATCH 4/8] adds tracking mission to planner --- .../missions/poop.yaml | 8 + .../subjugator_mission_planner/setup.py | 2 +- .../mission_planner.py | 34 +- .../track_object_server.py | 338 ++++++++++++++++++ 4 files changed, 373 insertions(+), 9 deletions(-) create mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/poop.yaml create mode 100644 src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/track_object_server.py diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/poop.yaml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/poop.yaml new file mode 100644 index 00000000..894cb665 --- /dev/null +++ b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/poop.yaml @@ -0,0 +1,8 @@ +mission: + - task: "track_object" + parameters: + object_type: "green_square" + type_of_movement: "rotate" + max_duration: 120.0 + desired_distance: 2.0 + keep_centered: true diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.py index 5355de61..af18ab08 100644 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.py +++ b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.py @@ -17,7 +17,6 @@ ), (f"share/{package_name}", ["package.xml"]), (f"share/{package_name}/missions", glob("missions/*.yaml")), - (f"share/{package_name}/missions", ["missions/prequal.yaml"]), ( f"share/{package_name}/launch", ["launch/mission_planner_launch.py", "launch/task_server_launch.py"], @@ -39,6 +38,7 @@ "search_server = subjugator_mission_planner.search_server:main", "movement_server = subjugator_mission_planner.movement_server:main", "wait_server = subjugator_mission_planner.wait_server:main", + "track_object = subjugator_mission_planner.track_object_server:main", ], }, ) diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mission_planner.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mission_planner.py index 89e67e6c..99976ede 100644 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mission_planner.py +++ b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mission_planner.py @@ -4,12 +4,13 @@ import yaml from ament_index_python.packages import get_package_share_directory from geometry_msgs.msg import Pose -from rclpy.action import ActionClient +from rclpy.action.client import ActionClient from rclpy.node import Node from subjugator_msgs.action import ( Movement, NavigateAround, Wait, + TrackObject, ) @@ -19,8 +20,7 @@ def __init__(self): package_share = get_package_share_directory("subjugator_mission_planner") print(f"PATH: {package_share}") - - default_mission_file = os.path.join("prequal.yaml") + default_mission_file = os.path.join(package_share, "missions", "poop.yaml") self.declare_parameter("mission_file", default_mission_file) mission_file = os.path.join( package_share, @@ -48,6 +48,11 @@ def __init__(self): Wait, "wait", ) + self.track_object_client = ActionClient( + self, + TrackObject, + "track_object", + ) # Timer to periodically check mission progress and start tasks self.timer = self.create_timer(0.5, self.execute_mission) @@ -88,6 +93,8 @@ def execute_mission(self): self.send_navigate_around_goal(params) elif task_name == "wait": self.send_wait_goal(params) + elif task_name == "track_object": + self.send_track_object_goal(params) else: self.get_logger().error(f"Unknown task: {task_name}, skipping") self.current_task_index += 1 @@ -113,14 +120,25 @@ def send_move_to_goal(self, params): self.executing_task = True self._send_goal(self.movement_client, goal_msg) + def send_track_object_goal(self, params): + if not self.track_object_client.wait_for_server(timeout_sec=2.0): + self.get_logger().error("Track object goal no working") + return + + goal_msg = TrackObject.Goal() + goal_msg.object_type = "green" + goal_msg.type_of_movement = params.get("type_of_movement", "rotate") # either translate or rotate + + self.executing_task = True + self._send_goal(self.track_object_client, goal_msg) + def send_navigate_around_goal(self, params): - if not self.navigate_around_client.wait_for_server(timeout_sec=2.0): - self.get_logger().error("Navigate around action server not available") + if not self.track_object_client.wait_for_server(timeout_sec=2.0): + self.get_logger().error("track_object action server not available") return - goal_msg = NavigateAround.Goal() - goal_msg.object = params.get("object", "") - goal_msg.radius = params.get("radius", 0.0) + goal_msg = TrackObject.Goal() + goal_msg.object_type = "idk" self.executing_task = True self._send_goal(self.navigate_around_client, goal_msg) diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/track_object_server.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/track_object_server.py new file mode 100644 index 00000000..aa6a84ee --- /dev/null +++ b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/track_object_server.py @@ -0,0 +1,338 @@ +from rclpy.node import Node +import rclpy +from rclpy.action.server import ActionServer, CancelResponse, GoalResponse +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Pose + +from tf_transformations import quaternion_multiply, quaternion_from_euler + +from subjugator_msgs.action import TrackObject + +import time +import cv2 +import numpy as np + +# +---------------------+---------------------+ +# | DIRECTION | MOVEMENT | +# +---------------------+---------------------+ +# | -X | Move Left | +# | +X | Move Right | +# | -Y | Move Up | +# | +Y | Move Down | +# +---------------------+---------------------+ +def detect_lime_green_webcam(ret, frame): + if not ret: + print("Error: Could not read frame.") + return False, 0, 0, 0 + + # Get image dimensions + height, width = frame.shape[:2] + image_center_x = width // 2 + image_center_y = height // 2 + + # Convert to HSV color space + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + + # Define lime green color range in HSV + lower_green = np.array([35, 100, 100]) + upper_green = np.array([85, 255, 255]) + + # Create mask for lime green + mask = cv2.inRange(hsv, lower_green, upper_green) + + # Apply morphological operations to reduce noise + kernel = np.ones((5, 5), np.uint8) + mask = cv2.erode(mask, kernel, iterations=1) + mask = cv2.dilate(mask, kernel, iterations=2) + + # Find contours + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + # Default return values for if notin found + found = False + center_x = 0.0 + center_y = 0.0 + area = 0.0 + + # If contours found, get the largest one (assuming it's our target) + if contours: + largest_contour = max(contours, key=cv2.contourArea) + area = cv2.contourArea(largest_contour) + # Only proceed if the area is significant + if area > 100: # Minimum area threshold + # Calculate centroid of the contour + M = cv2.moments(largest_contour) + if M["m00"] > 0: # Avoid division by zero + cx = int(M["m10"] / M["m00"]) + cy = int(M["m01"] / M["m00"]) + # Convert to coordinates relative to center (-1.0 to 1.0) + center_x = (cx - image_center_x) / (width / 2.0) + center_y = (cy - image_center_y) / (height / 2.0) + found = True + + return found, center_x, center_y, area + + +class TrackObjectServer(Node): + def __init__(self): + super().__init__("track_object_server") + + # odom sub + self._odom_sub = self.create_subscription(Odometry, "odometry/filtered", self.odom_cb, 10) + self.last_odom = Odometry() + + # goal pub + self.goal_pub = self.create_publisher(Pose, "goal_pose", 10) + + # Action server + self._action_server = ActionServer( + self, + TrackObject, + "track_object", + execute_callback=self.execute_callback, + goal_callback=self.goal_callback, + cancel_callback=self.cancel_callback, + ) + + def init_camera(self): + self.cap = cv2.VideoCapture(0) + if not self.cap.isOpened(): + self.get_logger().error("Error: Could not open camera") + return + + time.sleep(1) + + def tear_down_camera(self): + self.cap.release() + + def translate_to_track(self, y_offset, z_offset) -> Pose | None: + y_movement_needed = abs(y_offset) > 0.1 + z_movement_needed = abs(z_offset) > 0.1 + + no_movement_needed = not (y_movement_needed or z_movement_needed) + if no_movement_needed: + return None + + # negative y? move left + delta_y = 0.3 if y_offset < 0 else -0.3 + delta_z = 0.15 if z_offset < 0 else -0.15 + + p = Pose() + p.position.x = self.last_odom.pose.pose.position.x + p.position.y = self.last_odom.pose.pose.position.y + p.position.z = self.last_odom.pose.pose.position.z + p.orientation.x = self.last_odom.pose.pose.orientation.x + p.orientation.y = self.last_odom.pose.pose.orientation.y + p.orientation.z = self.last_odom.pose.pose.orientation.z + p.orientation.w = self.last_odom.pose.pose.orientation.w + + p.position.y += delta_y + p.position.z += delta_z + + return p + + def rotate_to_track(self, y_offset, z_offset) -> Pose | None: + yaw_needed = abs(y_offset) > 0.1 + + if not yaw_needed: + return None + + # negative y? move left + delta_yaw = 0.1 if y_offset < 0 else -0.1 + + p = Pose() + p.position.x = self.last_odom.pose.pose.position.x + p.position.y = self.last_odom.pose.pose.position.y + p.position.z = self.last_odom.pose.pose.position.z + + # Get current orientation as a list + current_quat = [ + self.last_odom.pose.pose.orientation.x, + self.last_odom.pose.pose.orientation.y, + self.last_odom.pose.pose.orientation.z, + self.last_odom.pose.pose.orientation.w + ] + + # Create a quaternion representing the yaw increment + # Parameters are (roll, pitch, yaw) + yaw_quat = quaternion_from_euler(0, 0, delta_yaw) + + # Apply the yaw rotation to the current orientation + new_quat = quaternion_multiply(yaw_quat, current_quat) + + # Set the new orientation + p.orientation.x = new_quat[0] + p.orientation.y = new_quat[1] + p.orientation.z = new_quat[2] + p.orientation.w = new_quat[3] + + return p + + def odom_cb(self, msg: Odometry): + self.last_odom = msg + + def goal_callback(self, goal_request): + print(goal_request.object_type) + self.type_of_movement = goal_request.type_of_movement + return GoalResponse.ACCEPT + + def execute_callback(self, goal_handle): + print('hi') + self.init_camera() + + while True: + ret, frame = self.cap.read() + found, y_offset, z_offset, _ = detect_lime_green_webcam(ret, frame) + + if not found: + continue + + if self.type_of_movement == "translate": + p = self.translate_to_track(y_offset, z_offset) + elif self.type_of_movement == "rotate": + p = self.rotate_to_track(y_offset, z_offset) + else: + print("helpme") + + if p is not None: + self.goal_pub.publish(p) + + self.tear_down_camera() + + goal_handle.succeed() + result = TrackObject.Result() + result.success = True + result.message = "todo" + time.sleep(1.0) + return result + + def cancel_callback(self, goal_handle): + return CancelResponse.ACCEPT + +def main(): + rclpy.init() + node = TrackObjectServer() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == "__main__": + main() + +## below is for debugging only +def detect_lime_green_webcam_debug(): + """ + Opens webcam, detects lime green objects, shows the result, + and returns coordinates relative to center. + """ + # Open webcam + cap = cv2.VideoCapture(0) # 0 is usually the default webcam + if not cap.isOpened(): + print("Error: Could not open webcam.") + return False, 0, 0, 0 + + # Wait a moment for the camera to initialize + time.sleep(1) + + print("Press 'q' to quit...") + + while True: + # Read a frame from webcam + ret, frame = cap.read() + if not ret: + print("Error: Could not read frame.") + break + + # Get image dimensions + height, width = frame.shape[:2] + image_center_x = width // 2 + image_center_y = height // 2 + + # Convert to HSV color space + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + + # Define lime green color range in HSV + lower_green = np.array([35, 100, 100]) # Adjust these values for your specific lime green + upper_green = np.array([85, 255, 255]) + + # Create mask for lime green + mask = cv2.inRange(hsv, lower_green, upper_green) + + # Apply morphological operations to reduce noise + kernel = np.ones((5, 5), np.uint8) + mask = cv2.erode(mask, kernel, iterations=1) + mask = cv2.dilate(mask, kernel, iterations=2) + + # Find contours + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + # Default return values if nothing is found + found = False + center_x = 0.0 + center_y = 0.0 + area = 0.0 + + # Create a copy of the frame for visualization + display_frame = frame.copy() + + # Draw crosshair at center of the image + cv2.line(display_frame, (image_center_x - 20, image_center_y), + (image_center_x + 20, image_center_y), (0, 0, 255), 2) + cv2.line(display_frame, (image_center_x, image_center_y - 20), + (image_center_x, image_center_y + 20), (0, 0, 255), 2) + + # If contours found, get the largest one (assuming it's our target) + if contours: + largest_contour = max(contours, key=cv2.contourArea) + area = cv2.contourArea(largest_contour) + + # Only proceed if the area is significant + if area > 100: # Minimum area threshold + # Draw the contour + cv2.drawContours(display_frame, [largest_contour], -1, (0, 255, 0), 2) + + # Calculate centroid of the contour + M = cv2.moments(largest_contour) + if M["m00"] > 0: # Avoid division by zero + cx = int(M["m10"] / M["m00"]) + cy = int(M["m01"] / M["m00"]) + + # Convert to coordinates relative to center (-1.0 to 1.0) + center_x = (cx - image_center_x) / (width / 2.0) + center_y = (cy - image_center_y) / (height / 2.0) + + found = True + + # Draw centroid + cv2.circle(display_frame, (cx, cy), 7, (255, 0, 0), -1) + cv2.putText(display_frame, f"({center_x:.2f}, {center_y:.2f})", + (cx + 10, cy), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2) + + # Draw line from center to object + cv2.line(display_frame, (image_center_x, image_center_y), + (cx, cy), (255, 0, 0), 2) + + # Add text indicating detection status + status = "Tracking: YES" if found else "Tracking: NO" + cv2.putText(display_frame, status, (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255) if not found else (0, 255, 0), 2) + + # Show the mask (optional) + cv2.imshow('Mask', mask) + + # Show the result + cv2.imshow('Lime Green Detector', display_frame) + + # Print coordinates to console + if found: + print(f"Object at ({center_x:.2f}, {center_y:.2f}), area: {area:.1f}") + + # Check for key press + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + break + + # Clean up + cap.release() + cv2.destroyAllWindows() + + return found, center_x, center_y, area From 52eca812f7243ef1daecb7d0940d015549efd885 Mon Sep 17 00:00:00 2001 From: Joseph Handsome Date: Fri, 11 Jul 2025 13:27:18 -0400 Subject: [PATCH 5/8] rotates camera and does base_link -> odom --- .../track_object_server.py | 293 +++++++++++++----- 1 file changed, 216 insertions(+), 77 deletions(-) diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/track_object_server.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/track_object_server.py index aa6a84ee..7e373a95 100644 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/track_object_server.py +++ b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/track_object_server.py @@ -1,16 +1,17 @@ -from rclpy.node import Node +import time + +import cv2 +import numpy as np import rclpy -from rclpy.action.server import ActionServer, CancelResponse, GoalResponse -from nav_msgs.msg import Odometry from geometry_msgs.msg import Pose - -from tf_transformations import quaternion_multiply, quaternion_from_euler - +from nav_msgs.msg import Odometry +from rclpy.action.server import ActionServer, CancelResponse, GoalResponse +from rclpy.node import Node from subjugator_msgs.action import TrackObject +from tf_transformations import quaternion_from_euler, quaternion_multiply + +rotation_angle = 137 + 180 -import time -import cv2 -import numpy as np # +---------------------+---------------------+ # | DIRECTION | MOVEMENT | @@ -24,36 +25,59 @@ def detect_lime_green_webcam(ret, frame): if not ret: print("Error: Could not read frame.") return False, 0, 0, 0 - + + PADDING = 100 + + padded = cv2.copyMakeBorder( + frame, + top=PADDING, + bottom=PADDING, + left=PADDING, + right=PADDING, + borderType=cv2.BORDER_CONSTANT, + value=(0, 0, 0), # black + ) + + # Get new dimensions + (h, w) = padded.shape[:2] + center = (w // 2, h // 2) + + # Get rotation matrix + M = cv2.getRotationMatrix2D(center, rotation_angle, 1.0) + + # Rotate the padded image + rotated = cv2.warpAffine(padded, M, (w, h)) + frame = rotated + # Get image dimensions height, width = frame.shape[:2] image_center_x = width // 2 image_center_y = height // 2 - + # Convert to HSV color space hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) - + # Define lime green color range in HSV lower_green = np.array([35, 100, 100]) upper_green = np.array([85, 255, 255]) - + # Create mask for lime green mask = cv2.inRange(hsv, lower_green, upper_green) - + # Apply morphological operations to reduce noise kernel = np.ones((5, 5), np.uint8) mask = cv2.erode(mask, kernel, iterations=1) mask = cv2.dilate(mask, kernel, iterations=2) - + # Find contours contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - # Default return values for if notin found + + # Default return values for if nothing found found = False center_x = 0.0 center_y = 0.0 area = 0.0 - + # If contours found, get the largest one (assuming it's our target) if contours: largest_contour = max(contours, key=cv2.contourArea) @@ -69,7 +93,7 @@ def detect_lime_green_webcam(ret, frame): center_x = (cx - image_center_x) / (width / 2.0) center_y = (cy - image_center_y) / (height / 2.0) found = True - + return found, center_x, center_y, area @@ -78,12 +102,17 @@ def __init__(self): super().__init__("track_object_server") # odom sub - self._odom_sub = self.create_subscription(Odometry, "odometry/filtered", self.odom_cb, 10) + self._odom_sub = self.create_subscription( + Odometry, + "odometry/filtered", + self.odom_cb, + 10, + ) self.last_odom = Odometry() # goal pub self.goal_pub = self.create_publisher(Pose, "goal_pose", 10) - + # Action server self._action_server = ActionServer( self, @@ -95,7 +124,7 @@ def __init__(self): ) def init_camera(self): - self.cap = cv2.VideoCapture(0) + self.cap = cv2.VideoCapture(2) if not self.cap.isOpened(): self.get_logger().error("Error: Could not open camera") return @@ -126,8 +155,20 @@ def translate_to_track(self, y_offset, z_offset) -> Pose | None: p.orientation.z = self.last_odom.pose.pose.orientation.z p.orientation.w = self.last_odom.pose.pose.orientation.w - p.position.y += delta_y - p.position.z += delta_z + # vector in base_link of required translation + delta = [0, delta_y, delta_z] + rotation = [ + self.last_odom.pose.pose.orientation.w, + self.last_odom.pose.pose.orientation.x, + self.last_odom.pose.pose.orientation.y, + self.last_odom.pose.pose.orientation.z, + ] + + delta_in_odom = rotate_vector_by_quaternion(delta, rotation) + + p.position.x = delta_in_odom[0] + p.position.y = delta_in_odom[1] + p.position.z = delta_in_odom[2] return p @@ -150,16 +191,16 @@ def rotate_to_track(self, y_offset, z_offset) -> Pose | None: self.last_odom.pose.pose.orientation.x, self.last_odom.pose.pose.orientation.y, self.last_odom.pose.pose.orientation.z, - self.last_odom.pose.pose.orientation.w + self.last_odom.pose.pose.orientation.w, ] - + # Create a quaternion representing the yaw increment # Parameters are (roll, pitch, yaw) yaw_quat = quaternion_from_euler(0, 0, delta_yaw) - + # Apply the yaw rotation to the current orientation new_quat = quaternion_multiply(yaw_quat, current_quat) - + # Set the new orientation p.orientation.x = new_quat[0] p.orientation.y = new_quat[1] @@ -177,7 +218,7 @@ def goal_callback(self, goal_request): return GoalResponse.ACCEPT def execute_callback(self, goal_handle): - print('hi') + print("hi") self.init_camera() while True: @@ -197,6 +238,8 @@ def execute_callback(self, goal_handle): if p is not None: self.goal_pub.publish(p) + rclpy.spin_once(self, timeout_sec=0.01) + self.tear_down_camera() goal_handle.succeed() @@ -209,14 +252,44 @@ def execute_callback(self, goal_handle): def cancel_callback(self, goal_handle): return CancelResponse.ACCEPT -def main(): - rclpy.init() - node = TrackObjectServer() - rclpy.spin(node) - rclpy.shutdown() -if __name__ == "__main__": - main() +def inverse_quaternion(q): + # For unit quaternions, the inverse is the conjugate + # q = [a, b, c, d] (real part first) + return np.array([q[0], -q[1], -q[2], -q[3]]) + + +def rotate_vector_by_quaternion(v, q): + # v is the 3D vector [x, y, z] + # q is the quaternion [a, b, c, d] (real part first) + + # Create pure quaternion from vector (0, v) + v_quat = np.array([0, v[0], v[1], v[2]]) + + # Quaternion multiplication: q * v_quat * q^(-1) + q_inv = inverse_quaternion(q) + + # Helper function for quaternion multiplication + def quat_mult(q1, q2): + a1, b1, c1, d1 = q1 + a2, b2, c2, d2 = q2 + return np.array( + [ + a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2, + a1 * b2 + b1 * a2 + c1 * d2 - d1 * c2, + a1 * c2 - b1 * d2 + c1 * a2 + d1 * b2, + a1 * d2 + b1 * c2 - c1 * b2 + d1 * a2, + ], + ) + + # Apply rotation: q * v_quat + temp = quat_mult(q, v_quat) + # Complete rotation: (q * v_quat) * q^(-1) + result = quat_mult(temp, q_inv) + + # Extract vector part + return result[1:4] + ## below is for debugging only def detect_lime_green_webcam_debug(): @@ -225,114 +298,180 @@ def detect_lime_green_webcam_debug(): and returns coordinates relative to center. """ # Open webcam - cap = cv2.VideoCapture(0) # 0 is usually the default webcam + cap = cv2.VideoCapture(2) # 0 is usually the default webcam if not cap.isOpened(): print("Error: Could not open webcam.") return False, 0, 0, 0 - + # Wait a moment for the camera to initialize time.sleep(1) - + print("Press 'q' to quit...") - + while True: # Read a frame from webcam ret, frame = cap.read() if not ret: print("Error: Could not read frame.") break - + + PADDING = 100 + + padded = cv2.copyMakeBorder( + frame, + top=PADDING, + bottom=PADDING, + left=PADDING, + right=PADDING, + borderType=cv2.BORDER_CONSTANT, + value=(0, 0, 0), # black + ) + + # Get new dimensions + (h, w) = padded.shape[:2] + center = (w // 2, h // 2) + + # Get rotation matrix + M = cv2.getRotationMatrix2D(center, rotation_angle, 1.0) + + # Rotate the padded image + rotated = cv2.warpAffine(padded, M, (w, h)) + frame = rotated + # Get image dimensions height, width = frame.shape[:2] image_center_x = width // 2 image_center_y = height // 2 - + # Convert to HSV color space hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) - + # Define lime green color range in HSV - lower_green = np.array([35, 100, 100]) # Adjust these values for your specific lime green + lower_green = np.array( + [35, 100, 100], + ) # Adjust these values for your specific lime green upper_green = np.array([85, 255, 255]) - + # Create mask for lime green mask = cv2.inRange(hsv, lower_green, upper_green) - + # Apply morphological operations to reduce noise kernel = np.ones((5, 5), np.uint8) mask = cv2.erode(mask, kernel, iterations=1) mask = cv2.dilate(mask, kernel, iterations=2) - + # Find contours contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - + # Default return values if nothing is found found = False center_x = 0.0 center_y = 0.0 area = 0.0 - + # Create a copy of the frame for visualization display_frame = frame.copy() - + # Draw crosshair at center of the image - cv2.line(display_frame, (image_center_x - 20, image_center_y), - (image_center_x + 20, image_center_y), (0, 0, 255), 2) - cv2.line(display_frame, (image_center_x, image_center_y - 20), - (image_center_x, image_center_y + 20), (0, 0, 255), 2) - + cv2.line( + display_frame, + (image_center_x - 20, image_center_y), + (image_center_x + 20, image_center_y), + (0, 0, 255), + 2, + ) + cv2.line( + display_frame, + (image_center_x, image_center_y - 20), + (image_center_x, image_center_y + 20), + (0, 0, 255), + 2, + ) + # If contours found, get the largest one (assuming it's our target) if contours: largest_contour = max(contours, key=cv2.contourArea) area = cv2.contourArea(largest_contour) - + # Only proceed if the area is significant if area > 100: # Minimum area threshold # Draw the contour cv2.drawContours(display_frame, [largest_contour], -1, (0, 255, 0), 2) - + # Calculate centroid of the contour M = cv2.moments(largest_contour) if M["m00"] > 0: # Avoid division by zero cx = int(M["m10"] / M["m00"]) cy = int(M["m01"] / M["m00"]) - + # Convert to coordinates relative to center (-1.0 to 1.0) center_x = (cx - image_center_x) / (width / 2.0) center_y = (cy - image_center_y) / (height / 2.0) - + found = True - + # Draw centroid cv2.circle(display_frame, (cx, cy), 7, (255, 0, 0), -1) - cv2.putText(display_frame, f"({center_x:.2f}, {center_y:.2f})", - (cx + 10, cy), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2) - + cv2.putText( + display_frame, + f"({center_x:.2f}, {center_y:.2f})", + (cx + 10, cy), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + (255, 0, 0), + 2, + ) + # Draw line from center to object - cv2.line(display_frame, (image_center_x, image_center_y), - (cx, cy), (255, 0, 0), 2) - + cv2.line( + display_frame, + (image_center_x, image_center_y), + (cx, cy), + (255, 0, 0), + 2, + ) + # Add text indicating detection status status = "Tracking: YES" if found else "Tracking: NO" - cv2.putText(display_frame, status, (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255) if not found else (0, 255, 0), 2) - + cv2.putText( + display_frame, + status, + (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (0, 0, 255) if not found else (0, 255, 0), + 2, + ) + # Show the mask (optional) - cv2.imshow('Mask', mask) - + cv2.imshow("Mask", mask) + # Show the result - cv2.imshow('Lime Green Detector', display_frame) - + cv2.imshow("Lime Green Detector", display_frame) + # Print coordinates to console if found: print(f"Object at ({center_x:.2f}, {center_y:.2f}), area: {area:.1f}") - + # Check for key press key = cv2.waitKey(1) & 0xFF - if key == ord('q'): + if key == ord("q"): break - + # Clean up cap.release() cv2.destroyAllWindows() - + return found, center_x, center_y, area + + +def main(): + # detect_lime_green_webcam_debug() + rclpy.init() + node = TrackObjectServer() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == "__main__": + main() From 733ac73737fac4329a73e35312760e42d9c61414 Mon Sep 17 00:00:00 2001 From: Joseph Handsome Date: Fri, 11 Jul 2025 13:52:20 -0400 Subject: [PATCH 6/8] remove old goal to viz --- .../subjugator_bringup/launch/subjugator_setup.launch.py | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/subjugator/subjugator_bringup/launch/subjugator_setup.launch.py b/src/subjugator/subjugator_bringup/launch/subjugator_setup.launch.py index f93cd9bb..2ad62178 100644 --- a/src/subjugator/subjugator_bringup/launch/subjugator_setup.launch.py +++ b/src/subjugator/subjugator_bringup/launch/subjugator_setup.launch.py @@ -117,14 +117,6 @@ def generate_launch_description(): output="both", ) - joe_goal_to_rviz = Node( - package="goal_to_viz", - executable="goal_to_viz", - name="goal_to_viz", - output="both", - ) - - return LaunchDescription( [ gui_cmd, @@ -136,6 +128,5 @@ def generate_launch_description(): controller, path_planner, trajectory_planner, - joe_goal_to_rviz, ], ) From bad7a2031a5a7d6041e6b862bb2907ea53456628 Mon Sep 17 00:00:00 2001 From: Joseph Handsome Date: Fri, 11 Jul 2025 13:52:46 -0400 Subject: [PATCH 7/8] remove goal to viz --- .../goal_to_viz/goal_to_viz/__init__.py | 0 .../goal_to_viz/goal_to_viz/goal_to_viz.py | 50 ------------------- .../testing/goal_to_viz/package.xml | 18 ------- .../testing/goal_to_viz/resource/goal_to_viz | 0 src/subjugator/testing/goal_to_viz/setup.cfg | 4 -- src/subjugator/testing/goal_to_viz/setup.py | 26 ---------- 6 files changed, 98 deletions(-) delete mode 100644 src/subjugator/testing/goal_to_viz/goal_to_viz/__init__.py delete mode 100644 src/subjugator/testing/goal_to_viz/goal_to_viz/goal_to_viz.py delete mode 100644 src/subjugator/testing/goal_to_viz/package.xml delete mode 100644 src/subjugator/testing/goal_to_viz/resource/goal_to_viz delete mode 100644 src/subjugator/testing/goal_to_viz/setup.cfg delete mode 100644 src/subjugator/testing/goal_to_viz/setup.py diff --git a/src/subjugator/testing/goal_to_viz/goal_to_viz/__init__.py b/src/subjugator/testing/goal_to_viz/goal_to_viz/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subjugator/testing/goal_to_viz/goal_to_viz/goal_to_viz.py b/src/subjugator/testing/goal_to_viz/goal_to_viz/goal_to_viz.py deleted file mode 100644 index c9f55f89..00000000 --- a/src/subjugator/testing/goal_to_viz/goal_to_viz/goal_to_viz.py +++ /dev/null @@ -1,50 +0,0 @@ -from visualization_msgs.msg import Marker -from geometry_msgs.msg import Pose -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import Quaternion - -class ConverterNode(Node): - def __init__(self): - super().__init__("converter_node") - self.publisher = self.create_publisher(Marker, '/visualization_marker', 10) - self.sub = self.create_subscription(Pose, "/goal/trajectory", self.goal_cb, 10) - self.latest:Pose = Pose() - - def goal_cb(self, msg:Pose): - self.latest = msg - - marker = Marker() - marker.header.frame_id = "odom" - marker.header.stamp = self.get_clock().now().to_msg() - marker.ns = "orientation" - marker.id = 0 - marker.type = Marker.ARROW - marker.action = Marker.ADD - - marker.pose.position.x = msg.position.x - marker.pose.position.y = msg.position.y - marker.pose.position.z = msg.position.z - - # Orientation: rotate 90 degrees around Z (yaw = π/2) - marker.pose.orientation = Quaternion(x=msg.orientation.x, y=msg.orientation.y, z=msg.orientation.z, w=msg.orientation.w) - - marker.scale.x = 1.0 # Arrow length - marker.scale.y = 0.1 # Arrow width - marker.scale.z = 0.1 # Arrow height - - marker.color.r = 0.0 - marker.color.g = 1.0 - marker.color.b = 0.0 - marker.color.a = 1.0 - - self.publisher.publish(marker) - -def main(): - rclpy.init() - my_node = ConverterNode() - rclpy.spin(my_node) - rclpy.shutdown() - -if __name__ == "__main__": - main() diff --git a/src/subjugator/testing/goal_to_viz/package.xml b/src/subjugator/testing/goal_to_viz/package.xml deleted file mode 100644 index d1168e48..00000000 --- a/src/subjugator/testing/goal_to_viz/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - goal_to_viz - 0.0.0 - TODO: Package description - jh - TODO: License declaration - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/src/subjugator/testing/goal_to_viz/resource/goal_to_viz b/src/subjugator/testing/goal_to_viz/resource/goal_to_viz deleted file mode 100644 index e69de29b..00000000 diff --git a/src/subjugator/testing/goal_to_viz/setup.cfg b/src/subjugator/testing/goal_to_viz/setup.cfg deleted file mode 100644 index 2f8463e9..00000000 --- a/src/subjugator/testing/goal_to_viz/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/goal_to_viz -[install] -install_scripts=$base/lib/goal_to_viz diff --git a/src/subjugator/testing/goal_to_viz/setup.py b/src/subjugator/testing/goal_to_viz/setup.py deleted file mode 100644 index bcacd2bd..00000000 --- a/src/subjugator/testing/goal_to_viz/setup.py +++ /dev/null @@ -1,26 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'goal_to_viz' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='jh', - maintainer_email='nottellingu@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - "goal_to_viz = goal_to_viz.goal_to_viz:main" - ], - }, -) From ba7e0664569af95e3970125fec456058302f5dec Mon Sep 17 00:00:00 2001 From: Joseph Handsome Date: Fri, 11 Jul 2025 14:12:33 -0400 Subject: [PATCH 8/8] removes accidental changes to navigate around mission --- .../mission_planner.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mission_planner.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mission_planner.py index 99976ede..75cbb62e 100644 --- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mission_planner.py +++ b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mission_planner.py @@ -9,8 +9,8 @@ from subjugator_msgs.action import ( Movement, NavigateAround, - Wait, TrackObject, + Wait, ) @@ -127,18 +127,22 @@ def send_track_object_goal(self, params): goal_msg = TrackObject.Goal() goal_msg.object_type = "green" - goal_msg.type_of_movement = params.get("type_of_movement", "rotate") # either translate or rotate + goal_msg.type_of_movement = params.get( + "type_of_movement", + "rotate", + ) # either translate or rotate self.executing_task = True self._send_goal(self.track_object_client, goal_msg) def send_navigate_around_goal(self, params): - if not self.track_object_client.wait_for_server(timeout_sec=2.0): - self.get_logger().error("track_object action server not available") + if not self.navigate_around_client.wait_for_server(timeout_sec=2.0): + self.get_logger().error("Navigate around action server not available") return - goal_msg = TrackObject.Goal() - goal_msg.object_type = "idk" + goal_msg = NavigateAround.Goal() + goal_msg.object = params.get("object", "") + goal_msg.radius = params.get("radius", 0.0) self.executing_task = True self._send_goal(self.navigate_around_client, goal_msg)