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..aba066c 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 @@ -31,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', @@ -39,8 +54,43 @@ (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 +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', + '#', + 'TD', +] + KEYPOINT_DICT = { 'nose': 0, 'left_eye': 1, @@ -113,6 +163,20 @@ def __init__(self): 10, ) + self.is_paused_ = False + + self.start_camera_srv = self.create_service( + Trigger, + 'oakd/start_camera', + self.handle_start_camera + ) + + self.stop_camera_srv = self.create_service( + Trigger, + 'oakd/stop_camera', + self.handle_stop_camera + ) + timer_period = 0.0833 # seconds self.timer = self.create_timer(timer_period, self.pose_detect) @@ -133,6 +197,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 @@ -168,7 +235,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() @@ -197,22 +264,24 @@ 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: + + self.lights_blue_ = self.dir != Dir.STOP + + if self.dir_confirm >= 3: cmd_vel_msg = Twist() if self.dir == Dir.LEFT: cmd_vel_msg.angular.z = 0.4 @@ -278,6 +347,32 @@ 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.lights_blue_ = 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.lights_blue_ = False + 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)