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..a08124b
--- /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='unit02_simulation_motion_planner',
+ 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/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
diff --git a/projects/unit02_simulation_motion_planner/setup.py b/projects/unit02_simulation_motion_planner/setup.py
index d3fcb29..8b287dc 100644
--- a/projects/unit02_simulation_motion_planner/setup.py
+++ b/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',
],
},
)
diff --git a/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_2D_perception.py b/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_2D_perception.py
new file mode 100644
index 0000000..af3dcbb
--- /dev/null
+++ b/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()
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..53b7689
--- /dev/null
+++ b/projects/unit02_simulation_motion_planner/unit02_simulation_motion_planner/tb4_3D_perception.py
@@ -0,0 +1,131 @@
+import imp
+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
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import Point
+from std_msgs.msg import String
+
+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)
+
+ 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
+ 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))
+
+ 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)
+ #[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)
+ image_msg = self.bridge.cv2_to_imgmsg(image, 'bgr8')
+ self.publishers_.publish(image_msg)
+
+
+ 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()