Phase2 (b): Perception for TurtleBot 4 Open CV and MediaPipe: 3D Objects

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='unit02_simulation_motion_planner',
+            namespace='tb4_2D_perception',
+            executable='tb4_2D_perception',
+            name='turtlebot4_2D_perception'
+        )
+    ])

projects/unit02_simulation_motion_planner/package.xml

@@ -9,6 +9,10 @@
 
   <exec_depend>rclpy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>image_transport</exec_depend>
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>python3-opencv</exec_depend>
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

projects/unit02_simulation_motion_planner/setup.py

@@ -28,6 +28,8 @@
            'ekf = unit02_simulation_motion_planner.extented_kalman_filter:main',
            'tb4_goal = unit02_simulation_motion_planner.tb4_publish_goal:main',
            'tb4_undock = unit02_simulation_motion_planner.tb4_undock:main',
+           'tb4_2D_perception = unit02_simulation_motion_planner.tb4_2D_perception:main',
+           'tb4_3D_perception = unit02_simulation_motion_planner.tb4_3D_perception:main',
         ],
     },
 )

projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_2D_perception.py

@@ -0,0 +1,143 @@
+import rclpy
+from rclpy.node import Node 
+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 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
+
+            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()
+
+    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):
+    """
+    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()

projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_3D_perception.py

@@ -0,0 +1,121 @@
+import rclpy
+from rclpy.node import Node 
+from sensor_msgs.msg import Image
+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 TurtlePerception3D(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.
+        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):
+        """
+        TurtlePerception3D class constructor to initialize nodes,
+        subscribers, publishers and parameters
+        """
+        super().__init__('tb4_perception_3d')
+
+        # 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):
+        """
+        TurtlePerception3D class constructor to initialize nodes,
+        subscribers, publishers and parameters
+
+        Args: 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")
+
+            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,
+                                            max_num_objects=2,
+                                            min_detection_confidence=0.5,
+                                            min_tracking_confidence=0.8,
+                                            model_name='Shoe') as objectron:
+
+                start = time.time()
+
+                #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 TurtlePerception3D class to publish image
+    processed object detection
+
+    Args:
+        None
+    """
+    rclpy.init(args=args)
+    turtle_perception_3d = TurtlePerception3D()
+    rclpy.spin(turtle_perception_3d)
+    turtle_perception_3d.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()