Week 6 Line Following

Copy all the code below into your line_follower.py file. Then, review the annotations to understand how it all works.

line_follower.py

#!/usr/bin/env python3

import rospy
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image

from tb3 import Tb3Move

class LineFollower(object): # (1)!
    def __init__(self):
        node_name = "line_follower"
        rospy.init_node(node_name, anonymous=True)
        self.rate = rospy.Rate(5)

        self.cvbridge_interface = CvBridge()
        self.img_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.camera_cb)
        self.robot_controller = Tb3Move()

        self.ctrl_c = False
        rospy.on_shutdown(self.shutdown_ops)

    def shutdown_ops(self):
        self.robot_controller.stop()
        cv2.destroyAllWindows()
        self.ctrl_c = True

    def camera_cb(self, img_data):
        try:
            cv_img = self.cvbridge_interface.imgmsg_to_cv2(img_data, desired_encoding="bgr8")
        except CvBridgeError as e:
            print(e)

        _, width, _ = cv_img.shape # (2)!
        crop_width = 500
        crop_height = 40
        crop_x = int((width / 2) - (crop_width / 2))
        cropped_img = cv_img[10:crop_height, 10:crop_x+crop_width]

        hsv_img = cv2.cvtColor(cropped_img, cv2.COLOR_BGR2HSV)

        lower = (100, 100, 100)
        upper = (255, 255, 255)
        mask = cv2.inRange(hsv_img, lower, upper)
        res = cv2.bitwise_and(cropped_img, cropped_img, mask = mask)

        m = cv2.moments(mask)
        cy = m['m10'] / (m['m00'] + 1e-5) # (3)!
        cz = m['m01'] / (m['m00'] + 1e-5)

        cv2.circle(res, (int(cy), int(cz)), 10, (255, 0, 0), 2)
        cv2.imshow("filtered image", res)
        cv2.waitKey(1)

        y_error = cy - (width / 2) # (4)!

        kp = 1.0 / 50.0

        fwd_vel = 0.1 # (5)!
        ang_vel = kp * y_error # (6)!

        print(f"Y-error = {y_error:.3f} pixels, ang_vel = {ang_vel:.3f} rad/s")
        self.robot_controller.set_move_cmd(fwd_vel, ang_vel)
        self.robot_controller.{BLANK} # (7)!

    def main(self):
        while not self.ctrl_c:
            self.rate.sleep()

if __name__ == '__main__':
    lf_instance = LineFollower()
    try:
        lf_instance.main()
    except rospy.ROSInterruptException:
        pass

1. A lot of the things in here you will already be familiar with from the previous exercises, so we won't go into too much detail on all of this again. The main thing you will notice is that we have once again built a class structure around this, which should now be familiar to you from previous weeks of this course.

2. In this case we want our robot to follow the line that is printed on the floor. We do this by applying the same image processing steps as in the previous exercises, to isolate the colours associated with the line and calculate its location in the robot's viewpoint.

3. We'll use the centroid component cy to determine how far the robot needs to turn in order to keep the line in the centre of its vision:

4. We are implementing proportional control here.

   Ideally, we want the centre of the line on the floor to be in the centre of the robot's viewpoint at all times: this is our target position. The actual position is where the line on the floor actually is, i.e.: the cy centroid component. The position error is then the difference between the actual and target position:

5. The only way we can reduce this error is by changing the robot's angular velocity. The robot always needs to travel with forward velocity, so we define a fixed value at all times to achieve this.

6. In order to correct for our position error, we multiply it by a proportional gain (kp), which will provide us with an angular velocity that should start to make the error reduce.

   If the proportional gain is set appropriately, this should ensure that our position error (y_error) is always kept to a minimum, so that the robot follows the line!

7. We then simply set these two velocities in our robot_controller object and then publish them to the /cmd_vel topic using methods from the Tb3Move() class.

   Fill in the Blank!

   There is a method within the Tb3Move() class which allows us to publish a velocity command to the /cmd_vel topic. What is it? (Have a look at the tb3.py source code if you need a reminder).

← Back to Week 6 - Exercise 4