mbanov_3_2.py

import rclpy
from rclpy.node import Node
import cv2
import numpy as np
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from geometry_msgs.msg import Vector3, Twist


r_min = 0
g_min = 0
b_min = 0
r_max = 128
g_max = 128
b_max = 128
f = 265.23
u0 = 160.5


class MyNode(Node):
    def __init__(self):
        super().__init__('mbanov_3_2')
        self.create_subscription(Image, '/robot/camera/image', self.image_callback, 1)
        self.pub_twist = self.create_publisher(Twist, '/robot/cmd_vel', 1)

    def image_callback(self, data):
        frame = CvBridge().imgmsg_to_cv2(data)

        # Threshold the image - image is written in BGR format
        frame_threshold = cv2.inRange(frame, (b_min, g_min, r_min), (b_max, g_max, r_max))
        detected_pixels = np.transpose(np.nonzero(frame_threshold))

        if len(detected_pixels) == 0:  # BB-9 not detected
            self.pub_twist.publish(Twist())
            return

        u = np.mean(detected_pixels[:, 1])  # the middle point of the robot
        beta = np.arctan2(u0 - u, f)

        twist = Twist()
        twist.angular = Vector3(z=beta)
        self.pub_twist.publish(twist)


def main(args=None):
    rclpy.init(args=args)
    node = MyNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        rclpy.shutdown()


if __name__ == '__main__':
    main()