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()