From 89bf73f2108dd9ee2ea35b1a00c4b7c213ecb0ae Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Fri, 27 Sep 2024 09:00:26 -0400 Subject: [PATCH 1/7] Move the template into a new directory as part of the share/ installation target, look for assets using the share path not the absolute path of the file. Add start_camera, stop_camera services to start & stop the camera pipeline --- turtlebot4_vision_tutorials/setup.py | 1 + .../template_processing_script.py | 8 ++-- .../MovenetDepthaiEdge.py | 12 +++--- .../pose_detection.py | 42 +++++++++++++++++++ 4 files changed, 54 insertions(+), 9 deletions(-) rename turtlebot4_vision_tutorials/{turtlebot4_vision_tutorials => templates}/template_processing_script.py (97%) diff --git a/turtlebot4_vision_tutorials/setup.py b/turtlebot4_vision_tutorials/setup.py index 9956f9d..dac5423 100644 --- a/turtlebot4_vision_tutorials/setup.py +++ b/turtlebot4_vision_tutorials/setup.py @@ -16,6 +16,7 @@ (os.path.join('share', package_name, 'models'), glob('models/*.blob')), (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + (os.path.join('share', package_name, 'templates'), glob('templates/*.py')), ], install_requires=['setuptools'], zip_safe=True, diff --git a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/template_processing_script.py b/turtlebot4_vision_tutorials/templates/template_processing_script.py similarity index 97% rename from turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/template_processing_script.py rename to turtlebot4_vision_tutorials/templates/template_processing_script.py index 82a03f1..47d4d34 100644 --- a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/template_processing_script.py +++ b/turtlebot4_vision_tutorials/templates/template_processing_script.py @@ -90,14 +90,14 @@ def pd_postprocess(inference, crop_region): xnorm.append(xn) ynorm.append(yn) scores.append(inference[3*i+2]) - x.append(int(xmin + xn * size)) - y.append(int(ymin + yn * size)) - + x.append(int(xmin + xn * size)) + y.append(int(ymin + yn * size)) + next_crop_region = determine_crop_region(scores, x, y) if ${_smart_crop} else init_crop_region return x, y, xnorm, ynorm, scores, next_crop_region node.warn("Processing node started") -# Defines the default crop region (pads the full image from both sides to make it a square image) +# Defines the default crop region (pads the full image from both sides to make it a square image) # Used when the algorithm cannot reliably determine the crop region from the previous frame. init_crop_region = ${_init_crop_region} crop_region = init_crop_region diff --git a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/MovenetDepthaiEdge.py b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/MovenetDepthaiEdge.py index 382862d..18ef0e9 100644 --- a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/MovenetDepthaiEdge.py +++ b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/MovenetDepthaiEdge.py @@ -10,13 +10,16 @@ from turtlebot4_vision_tutorials.FPS import FPS -SCRIPT_DIR = Path(__file__).resolve().parent -MOVENET_LIGHTNING_MODEL = os.path.join(SCRIPT_DIR.parent, +from ament_index_python.packages import get_package_share_directory + +SHARE_DIR = get_package_share_directory('turtlebot4_vision_tutorials') +MOVENET_LIGHTNING_MODEL = os.path.join(SHARE_DIR, "models", "movenet_singlepose_lightning_U8_transpose.blob") -MOVENET_THUNDER_MODEL = os.path.join(SCRIPT_DIR.parent, +MOVENET_THUNDER_MODEL = os.path.join(SHARE_DIR, "models", "movenet_singlepose_thunder_U8_transpose.blob") +TEMPLATE_DIR = os.path.join(SHARE_DIR, 'templates') # Dictionary that maps from joint names to keypoint indices. KEYPOINT_DICT = { @@ -308,7 +311,7 @@ def build_processing_script(self): which is a python template ''' # Read the template - with open(os.path.join(SCRIPT_DIR, 'template_processing_script.py'), 'r') as file: + with open(os.path.join(TEMPLATE_DIR, 'template_processing_script.py'), 'r') as file: template = Template(file.read()) # Perform the substitution @@ -346,7 +349,6 @@ def pd_postprocess(self, inference): return body def next_frame(self): - self.fps.update() # Get the device camera frame if wanted diff --git a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py index ccf7b29..008ecb1 100644 --- a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py +++ b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py @@ -24,6 +24,7 @@ import rclpy from rclpy.node import Node from std_msgs.msg import String as string_msg +from std_srvs.srv import Trigger from sensor_msgs.msg import Image from irobot_create_msgs.msg import LightringLeds @@ -113,6 +114,20 @@ def __init__(self): 10, ) + self._is_paused = False + + self.start_camera_srv = self.create_service( + Trigger, + 'start_camera', + self.handle_start_camera + ) + + self.stop_camera_srv = self.create_service( + Trigger, + 'stop_camera', + self.handle_stop_camera + ) + timer_period = 0.0833 # seconds self.timer = self.create_timer(timer_period, self.pose_detect) @@ -133,6 +148,9 @@ def __init__(self): # self.button_1_function() def pose_detect(self): + if self._is_paused: + return + if not (POSE_DETECT_ENABLE): return # Run movenet on next frame @@ -278,6 +296,30 @@ def autonomous_lights(self): # Publish the message self.lightring_publisher.publish(lightring_msg) + def handle_start_camera(self, req, resp): + if self._is_paused: + self._is_paused = False + self.pose = MovenetDepthai(input_src='rgb', + model='thunder', + score_thresh=0.3, + crop=not 'store_true', + smart_crop=not 'store_true', + internal_frame_height=432) + resp.success = True + else: + resp.message = 'Device already running' + return resp + + def handle_stop_camera(self, req, resp): + if not self._is_paused: + self._is_paused = True + self.pose.device.close() + self.pose = None + resp.success = True + else: + resp.message = 'Device already stopped' + return resp + def main(args=None): rclpy.init(args=args) From 128ef0ed80b2c751cf0b19f6521d168c05099f6f Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Fri, 27 Sep 2024 12:04:44 -0400 Subject: [PATCH 2/7] Use the Semaphore alphabet for directions --- .../pose_detection.py | 39 ++++++++++++++++--- 1 file changed, 33 insertions(+), 6 deletions(-) diff --git a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py index 008ecb1..b2fbfb1 100644 --- a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py +++ b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py @@ -42,6 +42,33 @@ (1, 6): 'Y', (5, 6): 'Z', } +# Semaphore letters that indicate to drive left +LEFT_LETTERS = [ + 'B', + 'C', + 'H', + 'I', + 'O', + 'S', +] + +# Semaphore letters that indicate to drive right +RIGHT_LETTERS = [ + 'E', + 'F', + 'M', + 'W', + 'X', + 'Z', +] + +# Semaphore letters that indicate to drive forward +FORWARD_LETTERS = [ + 'T', + 'U', + '#' +] + KEYPOINT_DICT = { 'nose': 0, 'left_eye': 1, @@ -215,21 +242,21 @@ def angle_with_y(v): right_pose = int((right_arm_angle + 202.5) / 45) % 8 left_pose = int((left_arm_angle + 202.5) / 45) % 8 letter = SEMAPHORE_FLAG.get((right_pose, left_pose), None) + dir_temp = Dir.STOP - if right_pose == 2 and (left_pose > 3 and left_pose < 6): + if letter in LEFT_LETTERS: dir_temp = Dir.LEFT - self.lights_blue_ = True - elif left_pose == 6 and (right_pose > 2 and right_pose < 5): + elif letter in RIGHT_LETTERS: dir_temp = Dir.RIGHT - elif (left_pose > 6 or left_pose == 0) and right_pose < 2: + elif letter in FORWARD_LETTERS: dir_temp = Dir.STRAIGHT - if dir_temp != Dir.STOP: - self.lights_blue_ = True + if dir_temp == self.dir: self.dir_confirm += 1 else: self.dir_confirm = 1 self.dir = dir_temp + if self.dir_confirm > 4: cmd_vel_msg = Twist() if self.dir == Dir.LEFT: From bba4825910fc9a6975bd3aa89d76841171182ecd Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Fri, 27 Sep 2024 12:33:34 -0400 Subject: [PATCH 3/7] Reduce the number of confirmations needed --- .../turtlebot4_vision_tutorials/pose_detection.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py index b2fbfb1..395d318 100644 --- a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py +++ b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py @@ -213,7 +213,7 @@ def pose_detect(self): # Stamp the message with the current time flag_msg.data = letter self.semaphore_flag_publisher.publish(flag_msg) - self.get_logger().info('Letter detected is: ' + letter) + self.get_logger().info(f'Letter detected is: {letter}') self.autonomous_lights() @@ -257,7 +257,7 @@ def angle_with_y(v): self.dir_confirm = 1 self.dir = dir_temp - if self.dir_confirm > 4: + if self.dir_confirm >= 3: cmd_vel_msg = Twist() if self.dir == Dir.LEFT: cmd_vel_msg.angular.z = 0.4 From 060c836d4e942412e73d6264d340560ca10a1fcf Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Fri, 27 Sep 2024 12:37:55 -0400 Subject: [PATCH 4/7] Make the lights blue when the pose is recognized --- .../turtlebot4_vision_tutorials/pose_detection.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py index 395d318..f19a27b 100644 --- a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py +++ b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py @@ -257,6 +257,8 @@ def angle_with_y(v): self.dir_confirm = 1 self.dir = dir_temp + self.lights_blue_ = self.dir != Dir.STOP + if self.dir_confirm >= 3: cmd_vel_msg = Twist() if self.dir == Dir.LEFT: From 51bfc7ff7d4e383a886a03313ef17bd33cc9d89d Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Fri, 27 Sep 2024 13:08:27 -0400 Subject: [PATCH 5/7] Move the start/stop services into the oakd namespace --- .../turtlebot4_vision_tutorials/pose_detection.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py index f19a27b..9f94fc6 100644 --- a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py +++ b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py @@ -145,13 +145,13 @@ def __init__(self): self.start_camera_srv = self.create_service( Trigger, - 'start_camera', + 'oakd/start_camera', self.handle_start_camera ) self.stop_camera_srv = self.create_service( Trigger, - 'stop_camera', + 'oakd/stop_camera', self.handle_stop_camera ) From ea53799f91693d087237aa2a58e71c40b7afd8b4 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Fri, 27 Sep 2024 13:59:16 -0400 Subject: [PATCH 6/7] Clear the blue ring when stopping the camera --- .../turtlebot4_vision_tutorials/pose_detection.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py index 9f94fc6..694d099 100644 --- a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py +++ b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py @@ -141,7 +141,7 @@ def __init__(self): 10, ) - self._is_paused = False + self.is_paused_ = False self.start_camera_srv = self.create_service( Trigger, @@ -175,7 +175,7 @@ def __init__(self): # self.button_1_function() def pose_detect(self): - if self._is_paused: + if self.is_paused_: return if not (POSE_DETECT_ENABLE): @@ -326,8 +326,9 @@ def autonomous_lights(self): self.lightring_publisher.publish(lightring_msg) def handle_start_camera(self, req, resp): - if self._is_paused: - self._is_paused = False + if self.is_paused_: + self.is_paused_ = False + self.lights_blue_ = False self.pose = MovenetDepthai(input_src='rgb', model='thunder', score_thresh=0.3, @@ -340,8 +341,9 @@ def handle_start_camera(self, req, resp): return resp def handle_stop_camera(self, req, resp): - if not self._is_paused: - self._is_paused = True + if not self.is_paused_: + self.is_paused_ = True + self.lights_blue_ = False self.pose.device.close() self.pose = None resp.success = True From f51cbb5bd6c5430ad94cdb7f0f8dc529a3962684 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Mon, 30 Sep 2024 11:31:18 -0400 Subject: [PATCH 7/7] Add a comment block explaining the poses, add the space, numeral, cancel semaphore signals + "TD" referee signal (needed for the driving example) --- .../pose_detection.py | 24 ++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py index 694d099..aba066c 100644 --- a/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py +++ b/turtlebot4_vision_tutorials/turtlebot4_vision_tutorials/pose_detection.py @@ -32,6 +32,20 @@ POSE_DETECT_ENABLE = True +# keys: +# (right, left) where each value is the angle with the horiziontal +# quantized into 8 regions +# Human facing camera +# +# R 0 0 L +# 1 7 +# O +# 2 +--+--+ 6 +# | \|/ | +# 3 | +++ | 5 +# | | +# 4 | | 4 +# -+ +- SEMAPHORE_FLAG = { (4, 7): 'E', (4, 6): 'F', (4, 5): 'G', (2, 3): 'H', (3, 4): 'A', (2, 4): 'B', (1, 4): 'C', (0, 4): 'D', @@ -40,6 +54,13 @@ (2, 7): 'Q', (2, 6): 'R', (2, 5): 'S', (1, 0): 'T', (1, 7): 'U', (0, 5): 'V', (7, 6): 'W', (7, 5): 'X', (1, 6): 'Y', (5, 6): 'Z', + + # Extended semaphore signals + (0, 7): '#', (4, 4): ' ', (1, 5): 'DEL', + + # Not official semaphore, but allow the "touchdown" referee signal + # with both arms held vertically above the head + (0, 0): 'TD', } # Semaphore letters that indicate to drive left @@ -66,7 +87,8 @@ FORWARD_LETTERS = [ 'T', 'U', - '#' + '#', + 'TD', ] KEYPOINT_DICT = {