Solutions

line_follower.py

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

# msgs needed for /cmd_vel
from geometry_msgs.msg import Twist, Vector3

from sensor_msgs.msg import Image

import numpy as np
import cv2
import cv_bridge

class Follower(Node):

    def __init__(self):
        super().__init__('line_follower')
        
        # Create a bridge between ROS and OpenCV
        self.bridge = cv_bridge.CvBridge()
        
        
        # initalize the debugging window
        cv2.namedWindow("window", 1)
        
        
        # Create a default twist msg (all values 0).
        lin = Vector3()
        ang = Vector3()
        self.twist = Twist(linear=lin,angular=ang)
        
        
        # Publish msg to cmd_vel.
        self.cmd_vel_pub = self.create_publisher(
                Twist,      # message type
                '/cmd_vel',  # topic name
                10)          # history depth
        
        self.image_sub = self.create_subscription(
                Image,
                'camera1/image_raw',
                self.image_callback,  # function to run upon message arrival
                10)  # allows packet loss



    def image_callback(self,data):
        
        # converts the incoming ROS message to cv2 format and HSV (hue, saturation, value)
        image = self.bridge.imgmsg_to_cv2(data,desired_encoding='bgr8')
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

        # TODO: define the upper and lower bounds for what should be considered 'yellow'
        lower_yellow = np.array([ 10, 10, 10])
        upper_yellow = np.array([255, 255, 250])
        mask = cv2.inRange(hsv, lower_yellow, upper_yellow)

        # we now erase all pixels that aren't yellow
        h, w, d = image.shape
        search_top = int(3*h/4)
        search_bot = int(3*h/4 + 20)
        mask[0:search_top, 0:w] = 0
        mask[search_bot:h, 0:w] = 0

        # using moments() function, determine the center of the yellow pixels
        M = cv2.moments(mask)
        # if there are any yellow pixels found
        if M['m00'] > 0:
                # determine the center of the yellow pixels in the image
                cx = int(M['m10']/M['m00'])
                cy = int(M['m01']/M['m00'])

                # visualize a red circle in our debugging window to indicate
                # the center point of the yellow pixels
                cv2.circle(image, (cx, cy), 20, (0,0,255), -1)

                # TODO: based on the location of the line (approximated
                #       by the center of the yellow pixels), implement
                #       proportional control to have the robot follow
                #       the yellow line
                err = w/2 - cx
                k_p = 1.0 / 100.0
                self.twist.linear.x = 0.2
                self.twist.angular.z = k_p * err
                self.cmd_vel_pub.publish(self.twist)

        # show the debugging window
        cv2.imshow("window", image)
        cv2.waitKey(3)
            


def main(args=None):
    rclpy.init(args=args)
    pc_publisher = Follower()
    rclpy.spin(pc_publisher)
    pc_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()