adopted for 2324

cmp3103m_ros2_code_fragments/cmp3103m_ros2_code_fragments/colour_chaser.py

@@ -12,16 +12,18 @@
 from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
 import cv2
 import numpy as np
+from rclpy.executors import MultiThreadedExecutor
+
 
 class ColourChaser(Node):
     def __init__(self):
         super().__init__('colour_chaser')
 
         # publish cmd_vel topic to move the robot
-        self.pub_cmd_vel = self.create_publisher(Twist, 'cmd_vel', 10)
+        self.pub_cmd_vel = self.create_publisher(Twist, 'cmd_vel', 1)
 
         # subscribe to the camera topic
-        self.create_subscription(Image, '/camera/image_raw', self.camera_callback, 10)
+        self.create_subscription(Image, '/limo/depth_camera_link/image_raw', self.camera_callback, 1)
 
         # Used to convert between ROS and OpenCV images
         self.br = CvBridge()
@@ -46,7 +48,7 @@ def camera_callback(self, data):
         contours = sorted(contours, key=cv2.contourArea, reverse=True)[:1]
 
         # Draw contour(s) (image to draw on, contours, contour number -1 to draw all contours, colour, thickness):
-        current_frame_contours = cv2.drawContours(current_frame, contours, 0, (0, 255, 0), 20)
+        current_frame_contours = cv2.drawContours(current_frame, contours, 0, (255, 255, 0), 20)
 
         self.tw=Twist() # twist message to publish
 
@@ -57,19 +59,19 @@ def camera_callback(self, data):
                 # find the centroid of the contour
                 cx = int(M['m10']/M['m00'])
                 cy = int(M['m01']/M['m00'])
-                #print("Centroid of the biggest area: ({}, {})".format(cx, cy))
+                print("Centroid of the biggest area: ({}, {})".format(cx, cy))
 
                 # Draw a circle centered at centroid coordinates
                 # cv2.circle(image, center_coordinates, radius, color, thickness) -1 px will fill the circle
-                cv2.circle(current_frame, (round(cx), round(cy)), 50, (0, 255, 0), -1)
+                cv2.circle(current_frame, (round(cx), round(cy)), 5, (0, 255, 0), -1)
 
                 # find height/width of robot camera image from ros2 topic echo /camera/image_raw height: 1080 width: 1920
 
                 # if center of object is to the left of image center move left
-                if cx < 900:
+                if cx < data.width / 3:
                     self.tw.angular.z=0.3
                 # else if center of object is to the right of image center move right
-                elif cx >= 1200:
+                elif cx >= 2 * data.width / 3:
                     self.tw.angular.z=-0.3
                 else: # center of object is in a 100 px range in the center of the image so dont turn
                     #print("object in the center of image")
@@ -84,8 +86,8 @@ def camera_callback(self, data):
 
         # show the cv images
         current_frame_contours_small = cv2.resize(current_frame_contours, (0,0), fx=0.4, fy=0.4) # reduce image size
-        cv2.imshow("Image window", current_frame_contours_small)
-        cv2.waitKey(1)
+        #cv2.imshow("Image window", current_frame_contours_small)
+        #cv2.waitKey(1)
 
 def main(args=None):
     print('Starting colour_chaser.py.')

cmp3103m_ros2_code_fragments/cmp3103m_ros2_code_fragments/colour_chaser2.py

@@ -27,7 +27,7 @@ def __init__(self):
         self.timer = self.create_timer(timer_period, self.timer_callback)
 
         # subscribe to the camera topic
-        self.create_subscription(Image, '/camera/image_raw', self.camera_callback, 10)
+        self.create_subscription(Image, '/limo/depth_camera_link/image_raw', self.camera_callback, 10)
 
         # Used to convert between ROS and OpenCV images
         self.br = CvBridge()
@@ -47,12 +47,13 @@ def camera_callback(self, data):
         current_frame_mask = cv2.inRange(current_frame_hsv,(0, 150, 50), (255, 255, 255)) # orange
 
         contours, hierarchy = cv2.findContours(current_frame_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
-
+        #print(contours)
         # Sort by area (keep only the biggest one)
         contours = sorted(contours, key=cv2.contourArea, reverse=True)[:1]
+        #print(contours)
 
         # Draw contour(s) (image to draw on, contours, contour number -1 to draw all contours, colour, thickness):
-        current_frame_contours = cv2.drawContours(current_frame, contours, 0, (0, 255, 0), 20)
+        current_frame_contours = cv2.drawContours(current_frame, contours, 0, (255, 255, 0), 20)
 
         if len(contours) > 0:
             # find the centre of the contour: https://docs.opencv.org/3.4/d8/d23/classcv_1_1Moments.html
@@ -61,19 +62,19 @@ def camera_callback(self, data):
                 # find the centroid of the contour
                 cx = int(M['m10']/M['m00'])
                 cy = int(M['m01']/M['m00'])
-                #print("Centroid of the biggest area: ({}, {})".format(cx, cy))
+                print("Centroid of the biggest area: ({}, {})".format(cx, cy))
 
                 # Draw a circle centered at centroid coordinates
                 # cv2.circle(image, center_coordinates, radius, color, thickness) -1 px will fill the circle
-                cv2.circle(current_frame, (round(cx), round(cy)), 50, (0, 255, 0), -1)
+                cv2.circle(current_frame, (round(cx), round(cy)), 5, (0, 255, 0), -1)
 
                 # find height/width of robot camera image from ros2 topic echo /camera/image_raw height: 1080 width: 1920
 
                 # if center of object is to the left of image center move left
-                if cx < 900:
+                if cx < data.width / 3:
                     self.turn_vel = 0.3
                 # else if center of object is to the right of image center move right
-                elif cx >= 1200:
+                elif cx >= 2 * data.width / 3:
                     self.turn_vel = -0.3
                 else: # center of object is in a 100 px range in the center of the image so dont turn
                     #print("object in the center of image")

cmp3103m_ros2_code_fragments/cmp3103m_ros2_code_fragments/colour_contours.py

@@ -14,7 +14,7 @@
 class ColourContours(Node):
     def __init__(self):
         super().__init__('colour_contours')
-        self.create_subscription(Image, '/camera/image_raw', self.camera_callback, 10)
+        self.create_subscription(Image, '/limo/depth_camera_link/image_raw', self.camera_callback, 1)
 
         self.br = CvBridge()
 

cmp3103m_ros2_code_fragments/cmp3103m_ros2_code_fragments/colour_mask.py

@@ -16,18 +16,18 @@ class ColourMask(Node):
     def __init__(self):
         super().__init__('colour_mask')
 
-        # publish the output images of the OpenCV processing on seperate Image topics
+        # publish the output images of the OpenCV processing on separate Image topics
         self.pub_image_hsv = self.create_publisher(Image, 'image/hsv', 10)
         self.pub_image_mask = self.create_publisher(Image, 'image/mask', 10)
 
         # subscribe to the camera topic
-        self.create_subscription(Image, '/camera/image_raw', self.camera_callback, 10)
+        self.create_subscription(Image, '/limo/depth_camera_link/image_raw', self.camera_callback, 10)
 
         # Used to convert between ROS and OpenCV images
         self.br = CvBridge()
 
     def camera_callback(self, data):
-        #self.get_logger().info("camera_callback")
+        self.get_logger().info("camera_callback")
 
         # Convert ROS Image message to OpenCV image
         current_frame = self.br.imgmsg_to_cv2(data, desired_encoding='bgr8')

cmp3103m_ros2_code_fragments/cmp3103m_ros2_code_fragments/colour_mask2.py

@@ -16,7 +16,7 @@ def __init__(self):
         super().__init__('colour_mask')
         self.pub_video_hsv = self.create_publisher(Image, 'video/hsv', 10)
         self.pub_video_mask = self.create_publisher(Image, 'video/mask', 10)
-        self.create_subscription(Image, '/camera/image_raw', self.camera_callback, 10)
+        self.create_subscription(Image, '/limo/depth_camera_link/image_raw', self.camera_callback, 10)
 
         # Used to convert between ROS and OpenCV images
         self.br = CvBridge()