1. # import rclpy
    2. # from rclpy.node import Node
    3. # from sensor_msgs.msg import Image
    4. # import cv_bridge
    5. import time
    7. import numpy as np
    8. import cv2 as cv
    10. # from geometry_msgs.msg import Twist
    12. ## User-defined parameters: (Update these values to your liking)
    13. # Minimum size for a contour to be considered anything
    14. MIN_AREA = 500
    16. # Minimum size for a contour to be considered part of the track
    17. MIN_AREA_TRACK = 5000
    19. # Robot's speed when following the line
    20. LINEAR_SPEED = 0.2
    22. # Proportional constant to be applied on speed when turning
    23. # (Multiplied by the error value)
    24. KP = 1.5 / 100
    26. # If the line is completely lost, the error value shall be compensated by:
    27. LOSS_FACTOR = 1.2
    29. # Send messages every $TIMER_PERIOD seconds
    30. TIMER_PERIOD = 0.06
    32. # When about to end the track, move for ~$FINALIZATION_PERIOD more seconds
    33. FINALIZATION_PERIOD = 4
    35. # The maximum error value for which the robot is still in a straight line
    36. MAX_ERROR = 30
    38. # BGR values to filter only the selected color range
    39. lower_bgr_values = np.array([0, 0, 0])
    40. upper_bgr_values = np.array([180, 255, 46])
    42. finalization_countdown = None
    43. right_mark_count = 0
    45. just_seen_right_mark = None
    46. just_seen_line = None
    49. class MyNode:
    50.     def __init__(self):
    52.         self.crop_w_start = -1
    54.         # 订阅图像信息
    55.         self.image_input = cv.imread('img/black.jpg')
    56.         while True:
    57.             self.timer_callback()
    58.             time.sleep(0.06)
    60.     def crop_size(self, height, width):
    61.         """
    62.         裁剪感兴趣的区域
    63.         """
    64.         return (1 * height // 3, height, width // 4, 3 * width // 4)
    66.     def get_contour_data(self, mask, out):
    67.         """
    68.         Return the centroid of the largest contour in
    69.         the binary image 'mask' (the line)
    70.         and return the side in which the smaller camera_namecontour is (the track mark)
    71.         (If there are any of these contours),
    72.         and draw all contours on 'out' image
    73.         """
    74.         # 获取轮廓
    75.         contours, _ = cv.findContours(mask, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_NONE)
    77.         mark = {}
    78.         line = {}
    80.         for contour in contours:
    82.             M = cv.moments(contour)
    83.             # Search more about Image Moments on Wikipedia :)
    85.             if M['m00'] > MIN_AREA:
    86.                 # if countor.area > MIN_AREA:
    88.                 if (M['m00'] > MIN_AREA_TRACK):
    89.                     # Contour is part of the track
    90.                     line['x'] = self.crop_w_start + int(M["m10"] / M["m00"])
    91.                     line['y'] = int(M["m01"] / M["m00"])
    93.                     # plot the area in light blue
    94.                     cv.drawContours(out, contour, -1, (255, 255, 0), 1)
    95.                     cv.putText(out, str(M['m00']), (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])),
    96.                                cv.FONT_HERSHEY_PLAIN, 2, (255, 255, 0), 2)
    98.                 else:
    99.                     # Contour is a track mark
    100.                     if (not mark) or (mark['y'] > int(M["m01"] / M["m00"])):
    101.                         # if there are more than one mark, consider only
    102.                         # the one closest to the robot
    103.                         mark['y'] = int(M["m01"] / M["m00"])
    104.                         mark['x'] = self.crop_w_start + int(M["m10"] / M["m00"])
    106.                         # plot the area in pink
    107.                         cv.drawContours(out, contour, -1, (255, 0, 255), 1)
    108.                         cv.putText(out, str(M['m00']), (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])),
    109.                                    cv.FONT_HERSHEY_PLAIN, 2, (255, 0, 255), 2)
    111.         if mark and line:
    112.             # if both contours exist
    113.             if mark['x'] > line['x']:
    114.                 mark_side = "right"
    115.             else:
    116.                 mark_side = "left"
    117.         else:
    118.             mark_side = None
    120.         return (line, mark_side)
    122.     def timer_callback(self):
    124.         global error
    125.         # global image_input
    126.         global just_seen_line
    127.         global just_seen_right_mark
    128.         global should_move
    129.         global right_mark_count
    130.         global finalization_countdown
    132.         # Wait for the first image to be received
    133.         if type(self.image_input) != np.ndarray:
    134.             return
    135.         # 获取宽度和高度
    136.         height, width, _ = self.image_input.shape
    138.         image = self.image_input.copy()
    139.         # 裁剪感兴趣的区域
    140.         crop_h_start, crop_h_stop, self.crop_w_start, crop_w_stop = self.crop_size(height, width)
    142.         # 获取感兴趣的图片区域
    143.         crop = image[crop_h_start:crop_h_stop, self.crop_w_start:crop_w_stop]
    145.         # 获取二值化的图 非0表示直线
    146.         # (过滤颜色值 只看到轮廓)
    147.         mask = cv.inRange(crop, lower_bgr_values, upper_bgr_values)
    149.         # get the centroid of the biggest contour in the picture,
    150.         # and plot its detail on the cropped part of the output image
    151.         # 获取图片中最大轮廓的质心，
    152.         # 并在输出图像的裁剪部分绘制其细节
    153.         output = image
    154.         line, mark_side = self.get_contour_data(mask, output[crop_h_start:crop_h_stop, self.crop_w_start:crop_w_stop])
    155.         # also get the side in which the track mark "is"
    156.         print(f'line:{line}')
    157.         print(f'mask_side:{mark_side}')
    158.         # message = Twist()
    160.         if line:
    161.             # if there even is a line in the image:
    162.             # (as the camera could not be reading any lines)
    163.             x = line['x']
    165.             # error:= The difference between the center of the image
    166.             # and the center of the line
    167.             error = x - width // 2
    169.             # message.linear.x = LINEAR_SPEED
    170.             just_seen_line = True
    172.             # plot the line centroid on the image
    173.             cv.circle(output, (line['x'], crop_h_start + line['y']), 5, (0, 255, 0), 7)
    175.         else:
    176.             # There is no line in the image.
    177.             # Turn on the spot to find it again.
    178.             if just_seen_line:
    179.                 just_seen_line = False
    180.                 error = error * LOSS_FACTOR
    181.             # message.linear.x = 0.0
    183.         if mark_side != None:
    184.             print("mark_side: {}".format(mark_side))
    186.             if (mark_side == "right") and (finalization_countdown == None) and \
    187.                     (abs(error) <= MAX_ERROR) and (not just_seen_right_mark):
    189.                 right_mark_count += 1
    191.                 if right_mark_count > 1:
    192.                     # Start final countdown to stop the robot
    193.                     finalization_countdown = int(FINALIZATION_PERIOD / TIMER_PERIOD) + 1
    194.                     print("Finalization Process has begun!")
    196.                 just_seen_right_mark = True
    197.         else:
    198.             just_seen_right_mark = False
    200.         # Determine the speed to turn and get the line in the center of the camera.
    201.         # message.angular.z = float(error) * -KP
    202.         # print("Error: {} | Angular Z: {}, ".format(error, message.angular.z))
    204.         # Plot the boundaries where the image was cropped
    205.         cv.rectangle(output, (self.crop_w_start, crop_h_start), (crop_w_stop, crop_h_stop), (0, 0, 255), 2)
    207.         # Uncomment to show the binary picture
    208.         # cv2.imshow("mask", mask)
    210.         # Show the output image to the user
    211.         cv.imshow("output", output)
    212.         # Print the image for 5milis, then resume execution
    213.         cv.waitKey(5)
    215.         # Check for final countdown
    216.         if finalization_countdown != None:
    217.             if finalization_countdown > 0:
    218.                 finalization_countdown -= 1
    220.             elif finalization_countdown == 0:
    221.                 should_move = False
    224. if __name__ == '__main__':
    225.     MyNode()