From f2ede37623d4d2ca79ec0f021fa3e01f4972831e Mon Sep 17 00:00:00 2001 From: sumedhreddy90 Date: Thu, 14 Jul 2022 18:03:06 -0400 Subject: [PATCH 01/10] Add subcribers /color/image camera view --- .../tb4_perception.py | 45 +++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_perception.py diff --git a/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_perception.py b/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_perception.py new file mode 100644 index 0000000..52f8bcb --- /dev/null +++ b/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_perception.py @@ -0,0 +1,45 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +import cv2 # OpenCV library + +class TurtlePerception(Node): + """ + TurtlePerception class as a Node + """ + def __init__(self): + + super().__init__('tb4_perception') + + # subscriber to receive an Image from the /color/image topic. + self.subscription = self.create_subscription(Image,'/color/image', self.perception_callback, 10) + self.subscription + + # Initialize bridge between ROS2 and OpenCV + self.bridge = CvBridge() + + def perception_callback(self, frames_data): + + + self.get_logger().info('Receiving Turtlebot4 visual frames_data') + + # current frame + current_frame = self.bridge.imgmsg_to_cv2(frames_data) + + # Poping each and every frame + cv2.imshow("TurtleBot4 Camera View", current_frame) + + cv2.waitKey(1) + +def main(args=None): + + rclpy.init(args=args) + turtle_perception = TurtlePerception() + rclpy.spin(turtle_perception) + turtle_perception.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file From 114a01f67eebec4ed10aa2bbbddfe08158a5ae06 Mon Sep 17 00:00:00 2001 From: sumedhreddy90 Date: Thu, 14 Jul 2022 20:46:54 -0400 Subject: [PATCH 02/10] Add tb4_perception to setup.py --- projects/unit02_simulation_motion_planner/setup.py | 1 + 1 file changed, 1 insertion(+) diff --git a/projects/unit02_simulation_motion_planner/setup.py b/projects/unit02_simulation_motion_planner/setup.py index c187674..0d0614b 100644 --- a/projects/unit02_simulation_motion_planner/setup.py +++ b/projects/unit02_simulation_motion_planner/setup.py @@ -27,6 +27,7 @@ 'tb4_rider = unit02_simulation_motion_planner.tb4_ride:main', 'ekf = unit02_simulation_motion_planner.extented_kalman_filter:main', 'tb4_goal = unit02_simulation_motion_planner.tb4_publish_goal:main', + 'tb4_perception = unit02_simulation_motion_planner.tb4_perception:main', ], }, ) From 88c068e7269aa3495024485a7769df00f5dc7c11 Mon Sep 17 00:00:00 2001 From: sumedhreddy90 Date: Thu, 14 Jul 2022 20:47:20 -0400 Subject: [PATCH 03/10] Add launch tb4_perception.launch.py --- .../launch/tb4_perception.launch.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 projects/unit02_simulation_motion_planner/launch/tb4_perception.launch.py diff --git a/projects/unit02_simulation_motion_planner/launch/tb4_perception.launch.py b/projects/unit02_simulation_motion_planner/launch/tb4_perception.launch.py new file mode 100644 index 0000000..48cbda2 --- /dev/null +++ b/projects/unit02_simulation_motion_planner/launch/tb4_perception.launch.py @@ -0,0 +1,12 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='ros2_tb4_visual_nav', + namespace='tb4_perception', + executable='tb4_perception', + name='turtlebot4_ride_perception' + ) + ]) \ No newline at end of file From 850f2ba02ccbe2307ba5c138d53b0ce4f2997fb1 Mon Sep 17 00:00:00 2001 From: sumedhreddy90 Date: Thu, 14 Jul 2022 20:49:10 -0400 Subject: [PATCH 04/10] Add opencv to package.xml --- projects/unit02_simulation_motion_planner/package.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/projects/unit02_simulation_motion_planner/package.xml b/projects/unit02_simulation_motion_planner/package.xml index 444acf5..b7b5e4e 100644 --- a/projects/unit02_simulation_motion_planner/package.xml +++ b/projects/unit02_simulation_motion_planner/package.xml @@ -9,6 +9,10 @@ rclpy std_msgs + sensor_msgs + image_transport + cv_bridge + python3-opencv ament_copyright ament_flake8 ament_pep257 From 4f41ea18e41075bac2b9311bdc8aa00666098274 Mon Sep 17 00:00:00 2001 From: sumedhreddy90 Date: Thu, 14 Jul 2022 20:56:16 -0400 Subject: [PATCH 05/10] changes to launch --- .../launch/tb4_perception.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/projects/unit02_simulation_motion_planner/launch/tb4_perception.launch.py b/projects/unit02_simulation_motion_planner/launch/tb4_perception.launch.py index 48cbda2..3419e33 100644 --- a/projects/unit02_simulation_motion_planner/launch/tb4_perception.launch.py +++ b/projects/unit02_simulation_motion_planner/launch/tb4_perception.launch.py @@ -4,7 +4,7 @@ def generate_launch_description(): return LaunchDescription([ Node( - package='ros2_tb4_visual_nav', + package='unit02_simulation_motion_planner', namespace='tb4_perception', executable='tb4_perception', name='turtlebot4_ride_perception' From a24f5f28ce079c6381b5e251f6f30d698943b943 Mon Sep 17 00:00:00 2001 From: sumedhreddy90 Date: Fri, 15 Jul 2022 16:56:03 -0400 Subject: [PATCH 06/10] Add simple countour based detection --- .../tb4_perception.py | 139 +++++++++++++++--- 1 file changed, 117 insertions(+), 22 deletions(-) diff --git a/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_perception.py b/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_perception.py index 52f8bcb..b835d8c 100644 --- a/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_perception.py +++ b/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_perception.py @@ -3,43 +3,138 @@ from sensor_msgs.msg import Image from cv_bridge import CvBridge import cv2 # OpenCV library - +import numpy as np +from matplotlib import pyplot as plt + class TurtlePerception(Node): """ - TurtlePerception class as a Node + TurtlePerception class inherits from (or is a subclass of) Node + + Attributes: + Node: Is a class from rclpy.node.Node(node_name, *, context=None, + cli_args=None, namespace=None, use_global_arguments=True, + enable_rosout=True, start_parameter_services=True, parameter_overrides=None, + allow_undeclared_parameters=False, automatically_declare_parameters_from_overrides=False) + used to create a node, publish/ subscribe a node and access other ROS2 features + + More Information at: https://docs.ros2.org/latest/api/rclpy/api/node.html + + Topics: + - Publishers: None + - Subscribers: + - Topic name: /color/image + - Topic type: sensor_msgs.msg/Image + - Topic desciption: To subcribe image frames from OKA-D camera sensor of the TB4 """ def __init__(self): - + """ + TurtlePerception class constructor to initialize nodes, + subscribers, publishers and parameters + """ super().__init__('tb4_perception') - + # subscriber to receive an Image from the /color/image topic. - self.subscription = self.create_subscription(Image,'/color/image', self.perception_callback, 10) - self.subscription - + self.subscription = self.create_subscription(Image,'/color/image', + self.perception_callback, 10) + # Initialize bridge between ROS2 and OpenCV self.bridge = CvBridge() - + def perception_callback(self, frames_data): + """ + TurtlePerception class constructor to initialize nodes, + subscribers, publishers and parameters - + Args: frames_data + """ self.get_logger().info('Receiving Turtlebot4 visual frames_data') - + # current frame current_frame = self.bridge.imgmsg_to_cv2(frames_data) - + # Poping each and every frame cv2.imshow("TurtleBot4 Camera View", current_frame) - + + process_frames = current_frame.copy() + + # converting image into grayscale image + gray = cv2.cvtColor(process_frames, cv2.COLOR_BGR2GRAY) + + # setting threshold of gray image + _, threshold = cv2.threshold(gray, 127, 255, cv2.THRESH_BINARY) + + # using a findContours() function + + # The function retrieves contours from the binary image using the algorithm + # # (Satoshi Suzuki and others.Topological structural analysis of digitized binary images + # by border following Computer Vision, Graphics, and Image Processing, 30(1):32–46, 1985.) + # The contours are a useful tool for shape analysis and object detection and recognition. + + contours, _ = cv2.findContours( + threshold, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + + i = 0 + + # list for storing names of shapes + for contour in contours: + + # here we are ignoring first counter because + # findcontour function detects whole image as shape + if i == 0: + i = 1 + continue + + # cv2.approxPloyDP() function to approximate the shape + approx = cv2.approxPolyDP( + contour, 0.01 * cv2.arcLength(contour, True), True) + + # using drawContours() function + cv2.drawContours(process_frames, [contour], 0, (0, 0, 255), 5) + + # finding center point of shape + cv_moments = cv2.moments(contour) + if cv_moments['m00'] != 0.0: + x_point = int(cv_moments['m10']/cv_moments['m00']) + y_point = int(cv_moments['m01']/cv_moments['m00']) + + # putting shape name at center of each shape + if len(approx) == 3: + cv2.putText(process_frames, 'Triangle', (x_point, y_point), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2) + + elif len(approx) == 4: + cv2.putText(process_frames, 'Quadrilateral', (x_point, y_point), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2) + + elif len(approx) == 5: + cv2.putText(process_frames, 'Pentagon', (x_point, y_point), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2) + + elif len(approx) == 6: + cv2.putText(process_frames, 'Hexagon', (x_point, y_point), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2) + + else: + cv2.putText(process_frames, 'circle', (x_point, y_point), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2) + + # displaying the image after drawing contours + cv2.imshow('Turtlebot 4 Simple Object Detection', process_frames) cv2.waitKey(1) - + def main(args=None): - - rclpy.init(args=args) - turtle_perception = TurtlePerception() - rclpy.spin(turtle_perception) - turtle_perception.destroy_node() - rclpy.shutdown() - - + """ + Main method to instantiate ROS nodes and TurtlePerception class to publish image + processed object detection + + Args: + None + """ + rclpy.init(args=args) + turtle_perception = TurtlePerception() + rclpy.spin(turtle_perception) + turtle_perception.destroy_node() + rclpy.shutdown() + if __name__ == '__main__': - main() \ No newline at end of file + main() From 775264f0caced35c997ca827b158018f6eeeabb7 Mon Sep 17 00:00:00 2001 From: sumedhreddy90 Date: Mon, 25 Jul 2022 09:59:39 -0400 Subject: [PATCH 07/10] Add classical computer vision 2D shape detection pipeline --- .../launch/tb4_perception.launch.py | 6 +++--- .../unit02_simulation_motion_planner/setup.py | 2 +- .../{tb4_perception.py => tb4_2D_perception.py} | 15 +++++++++------ 3 files changed, 13 insertions(+), 10 deletions(-) rename projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/{tb4_perception.py => tb4_2D_perception.py} (87%) diff --git a/projects/unit02_simulation_motion_planner/launch/tb4_perception.launch.py b/projects/unit02_simulation_motion_planner/launch/tb4_perception.launch.py index 3419e33..a08124b 100644 --- a/projects/unit02_simulation_motion_planner/launch/tb4_perception.launch.py +++ b/projects/unit02_simulation_motion_planner/launch/tb4_perception.launch.py @@ -5,8 +5,8 @@ def generate_launch_description(): return LaunchDescription([ Node( package='unit02_simulation_motion_planner', - namespace='tb4_perception', - executable='tb4_perception', - name='turtlebot4_ride_perception' + namespace='tb4_2D_perception', + executable='tb4_2D_perception', + name='turtlebot4_2D_perception' ) ]) \ No newline at end of file diff --git a/projects/unit02_simulation_motion_planner/setup.py b/projects/unit02_simulation_motion_planner/setup.py index a89203a..2b979a5 100644 --- a/projects/unit02_simulation_motion_planner/setup.py +++ b/projects/unit02_simulation_motion_planner/setup.py @@ -27,7 +27,7 @@ 'tb4_rider = unit02_simulation_motion_planner.tb4_planner:main', 'ekf = unit02_simulation_motion_planner.extented_kalman_filter:main', 'tb4_goal = unit02_simulation_motion_planner.tb4_publish_goal:main', - 'tb4_perception = unit02_simulation_motion_planner.tb4_perception:main', + 'tb4_2D_perception = unit02_simulation_motion_planner.tb4_2D_perception:main', ], }, ) diff --git a/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_perception.py b/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_2D_perception.py similarity index 87% rename from projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_perception.py rename to projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_2D_perception.py index b835d8c..af3dcbb 100644 --- a/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_perception.py +++ b/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_2D_perception.py @@ -8,14 +8,17 @@ class TurtlePerception(Node): """ - TurtlePerception class inherits from (or is a subclass of) Node + - TurtlePerception class inherits from (or is a subclass of) Node attributes as a ROS Node + that acts as a primary entrypoint in the ROS system for communication. It can be used to + create ROS entities such as publishers, subscribers, services, etc. + - This class is an implementation of the 2D object detection using classical openCV. + Here, we use inbuilt opencv methods such as + - cv2.findContours(): to retrieve contours from the binary image + - cv2.approxPloyDP(): to approximate the shape detected + - cv2.drawContours(): to draw bounding boxes for detected objects Attributes: - Node: Is a class from rclpy.node.Node(node_name, *, context=None, - cli_args=None, namespace=None, use_global_arguments=True, - enable_rosout=True, start_parameter_services=True, parameter_overrides=None, - allow_undeclared_parameters=False, automatically_declare_parameters_from_overrides=False) - used to create a node, publish/ subscribe a node and access other ROS2 features + Node: Is a class from rclpy.node.Node More Information at: https://docs.ros2.org/latest/api/rclpy/api/node.html From 8b2f454ede2bd3bed7df08718541294c290dccb7 Mon Sep 17 00:00:00 2001 From: sumedhreddy90 Date: Mon, 25 Jul 2022 10:52:41 -0400 Subject: [PATCH 08/10] Add 3D object detection Stub CV MediaPipe --- .../tb4_3D_perception.py | 88 +++++++++++++++++++ 1 file changed, 88 insertions(+) create mode 100644 projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_3D_perception.py diff --git a/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_3D_perception.py b/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_3D_perception.py new file mode 100644 index 0000000..3d9e47e --- /dev/null +++ b/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_3D_perception.py @@ -0,0 +1,88 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +import cv2 # OpenCV library +import mediapipe as mp +import numpy as np +from matplotlib import pyplot as plt + +class TurtlePerception(Node): + """ + - TurtlePerception class inherits from (or is a subclass of) Node attributes as a ROS Node + that acts as a primary entrypoint in the ROS system for communication. It can be used to + create ROS entities such as publishers, subscribers, services, etc. + - This class is an implementation of the 3D object detection using openCV and MediaPipe. + Here, we use inbuilt opencv methods such as + - TODO + + + Attributes: + Node: Is a class from rclpy.node.Node + + More Information at: https://docs.ros2.org/latest/api/rclpy/api/node.html + + Topics: + - Publishers: None + - Subscribers: + - Topic name: /color/image + - Topic type: sensor_msgs.msg/Image + - Topic desciption: To subcribe image frames from OKA-D camera sensor of the TB4 + """ + def __init__(self): + """ + TurtlePerception class constructor to initialize nodes, + subscribers, publishers and parameters + """ + super().__init__('tb4_perception') + + # subscriber to receive an Image from the /color/image topic. + self.subscription = self.create_subscription(Image,'/color/image', + self.perception_callback, 10) + + # Initialize bridge between ROS2 and OpenCV + self.bridge = CvBridge() + self.mp_objectron = mp.solutions.objectron + self.mp_drawing = mp.solutions.drawing_utils + + def perception_callback(self, frames_data): + """ + TurtlePerception class constructor to initialize nodes, + subscribers, publishers and parameters + + Args: frames_data + """ + self.get_logger().info('Receiving Turtlebot4 visual frames_data') + + # current frame + current_frame = self.bridge.imgmsg_to_cv2(frames_data, desired_encoding="bgr8") + + resized_image = cv2.resize(current_frame, (360, 640)) + + # Poping each and every frame + cv2.imshow("TurtleBot4 Camera View", resized_image) + + process_frames = resized_image.copy() + + with self.mp_objectron.Objectron() + + # displaying the image after drawing contours + cv2.imshow('Turtlebot 4 Simple Object Detection', process_frames) + cv2.waitKey(1) + +def main(args=None): + """ + Main method to instantiate ROS nodes and TurtlePerception class to publish image + processed object detection + + Args: + None + """ + rclpy.init(args=args) + turtle_perception = TurtlePerception() + rclpy.spin(turtle_perception) + turtle_perception.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() From 25ac12765436cfe18c8de531bf3fd51fcef2af84 Mon Sep 17 00:00:00 2001 From: sumedhreddy90 Date: Mon, 25 Jul 2022 12:22:40 -0400 Subject: [PATCH 09/10] 3D object detection using Open CV and Mediapipe --- .../unit02_simulation_motion_planner/setup.py | 1 + .../tb4_3D_perception.py | 75 +++++++++++++------ 2 files changed, 55 insertions(+), 21 deletions(-) diff --git a/projects/unit02_simulation_motion_planner/setup.py b/projects/unit02_simulation_motion_planner/setup.py index 2b979a5..9391195 100644 --- a/projects/unit02_simulation_motion_planner/setup.py +++ b/projects/unit02_simulation_motion_planner/setup.py @@ -28,6 +28,7 @@ 'ekf = unit02_simulation_motion_planner.extented_kalman_filter:main', 'tb4_goal = unit02_simulation_motion_planner.tb4_publish_goal:main', 'tb4_2D_perception = unit02_simulation_motion_planner.tb4_2D_perception:main', + 'tb4_3D_perception = unit02_simulation_motion_planner.tb4_3D_perception:main', ], }, ) diff --git a/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_3D_perception.py b/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_3D_perception.py index 3d9e47e..d37ac5a 100644 --- a/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_3D_perception.py +++ b/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_3D_perception.py @@ -1,15 +1,16 @@ import rclpy from rclpy.node import Node from sensor_msgs.msg import Image -from cv_bridge import CvBridge +from cv_bridge import CvBridge, CvBridgeError import cv2 # OpenCV library import mediapipe as mp import numpy as np from matplotlib import pyplot as plt +import time -class TurtlePerception(Node): +class TurtlePerception3D(Node): """ - - TurtlePerception class inherits from (or is a subclass of) Node attributes as a ROS Node + - TurtlePerception3D class inherits from (or is a subclass of) Node attributes as a ROS Node that acts as a primary entrypoint in the ROS system for communication. It can be used to create ROS entities such as publishers, subscribers, services, etc. - This class is an implementation of the 3D object detection using openCV and MediaPipe. @@ -31,10 +32,10 @@ class TurtlePerception(Node): """ def __init__(self): """ - TurtlePerception class constructor to initialize nodes, + TurtlePerception3D class constructor to initialize nodes, subscribers, publishers and parameters """ - super().__init__('tb4_perception') + super().__init__('tb4_perception_3d') # subscriber to receive an Image from the /color/image topic. self.subscription = self.create_subscription(Image,'/color/image', @@ -47,41 +48,73 @@ def __init__(self): def perception_callback(self, frames_data): """ - TurtlePerception class constructor to initialize nodes, + TurtlePerception3D class constructor to initialize nodes, subscribers, publishers and parameters Args: frames_data """ - self.get_logger().info('Receiving Turtlebot4 visual frames_data') + try: + self.get_logger().info('Receiving Turtlebot4 visual frames_data') - # current frame - current_frame = self.bridge.imgmsg_to_cv2(frames_data, desired_encoding="bgr8") + # current frame + current_frame = self.bridge.imgmsg_to_cv2(frames_data, desired_encoding="bgr8") - resized_image = cv2.resize(current_frame, (360, 640)) + resized_image = cv2.resize(current_frame, (720, 480)) - # Poping each and every frame - cv2.imshow("TurtleBot4 Camera View", resized_image) + # Poping each and every frame + cv2.imshow("TurtleBot4 Camera View", resized_image) - process_frames = resized_image.copy() + process_frames = resized_image.copy() - with self.mp_objectron.Objectron() + with self.mp_objectron.Objectron(static_image_mode=False, + max_num_objects=2, + min_detection_confidence=0.5, + min_tracking_confidence=0.8, + model_name='Shoe') as objectron: + + start = time.time() - # displaying the image after drawing contours - cv2.imshow('Turtlebot 4 Simple Object Detection', process_frames) - cv2.waitKey(1) + #Convert the BGR image to RGB + image = cv2.cvtColor(process_frames, cv2.COLOR_BGR2RGB) + + # Improving performace by marking the image as not writeable to pass by reference + image.flags.writeable = False + + # 3D object detection + detected_object_frame = objectron.process(image) + + image.flags.writeable = True + image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR) + + if detected_object_frame.detected_objects: + for detected_objects in detected_object_frame.detected_objects: + self.mp_drawing.draw_landmarks(image, detected_objects.landmarks_2d, self.mp_objectron.BOX_CONNECTIONS) + self.mp_drawing.draw_axis(image, detected_objects.rotation, detected_objects.translation) + + end = time.time() + total_time = end - start + + fps = 1 / total_time + cv2.putText(image, f'FPS: {int(fps)}', (20,70), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0,255,0), 2) + # displaying the image after drawing contours + cv2.imshow('Turtlebot 4 Simple Object Detection', image) + cv2.waitKey(1) + + except CvBridgeError as e: + print(e) def main(args=None): """ - Main method to instantiate ROS nodes and TurtlePerception class to publish image + Main method to instantiate ROS nodes and TurtlePerception3D class to publish image processed object detection Args: None """ rclpy.init(args=args) - turtle_perception = TurtlePerception() - rclpy.spin(turtle_perception) - turtle_perception.destroy_node() + turtle_perception_3d = TurtlePerception3D() + rclpy.spin(turtle_perception_3d) + turtle_perception_3d.destroy_node() rclpy.shutdown() if __name__ == '__main__': From cf190bd39a6c26d00dcdae3571b05a6261b73253 Mon Sep 17 00:00:00 2001 From: Sumedh Reddy Date: Fri, 19 Aug 2022 17:59:43 -0400 Subject: [PATCH 10/10] add publishers --- .../tb4_3D_perception.py | 24 +++++++++++++------ 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_3D_perception.py b/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_3D_perception.py index d37ac5a..53b7689 100644 --- a/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_3D_perception.py +++ b/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_3D_perception.py @@ -1,3 +1,4 @@ +import imp import rclpy from rclpy.node import Node from sensor_msgs.msg import Image @@ -7,6 +8,9 @@ import numpy as np from matplotlib import pyplot as plt import time +from geometry_msgs.msg import Pose +from geometry_msgs.msg import Point +from std_msgs.msg import String class TurtlePerception3D(Node): """ @@ -41,6 +45,10 @@ def __init__(self): self.subscription = self.create_subscription(Image,'/color/image', self.perception_callback, 10) + self.publishers_ = self.create_publisher(Image,'perception/detected/image', 10) + self.publisher_detected_pose = self.create_publisher(Pose,'perception/detected/pose', 10) + self.detected_pose = Pose() + # Initialize bridge between ROS2 and OpenCV self.bridge = CvBridge() self.mp_objectron = mp.solutions.objectron @@ -61,9 +69,6 @@ def perception_callback(self, frames_data): resized_image = cv2.resize(current_frame, (720, 480)) - # Poping each and every frame - cv2.imshow("TurtleBot4 Camera View", resized_image) - process_frames = resized_image.copy() with self.mp_objectron.Objectron(static_image_mode=False, @@ -90,15 +95,20 @@ def perception_callback(self, frames_data): for detected_objects in detected_object_frame.detected_objects: self.mp_drawing.draw_landmarks(image, detected_objects.landmarks_2d, self.mp_objectron.BOX_CONNECTIONS) self.mp_drawing.draw_axis(image, detected_objects.rotation, detected_objects.translation) + #[TODO] Covert detected_objects.rotation and detected_objects.translation from np.ndarry + # to ROS supportable msg + self.publisher_detected_pose.publish(self.detected_pose) + + end = time.time() total_time = end - start - + fps = 1 / total_time cv2.putText(image, f'FPS: {int(fps)}', (20,70), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0,255,0), 2) - # displaying the image after drawing contours - cv2.imshow('Turtlebot 4 Simple Object Detection', image) - cv2.waitKey(1) + image_msg = self.bridge.cv2_to_imgmsg(image, 'bgr8') + self.publishers_.publish(image_msg) + except CvBridgeError as e: print(e)