Line Follower - Not Following!

edit

Hi! I am trying to adapt this following code to a Leo Rover that has a color camera and will follow a blue line by filtering and finding the centroid with OpenCV. When I run follow_line_step_hsv.py, I get nothing but a blinking cursor. The Rover does not move and I can't seem to find the reason.

The first scrip Move_Robot.PY seems to work and is as follows:

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist

class MoveKobuki(object):

def __init__(self):

    self.cmd_vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)
    self.cmd_vel_subs = rospy.Subscriber("cmd_vel", Twist, self.cmdvel_callback)
    self.last_cmdvel_command = Twist()
    self._cmdvel_pub_rate = rospy.Rate(10)
    self.shutdown_detected = False

def cmdvel_callback(self, msg):
    self.last_cmdvel_command = msg

def compare_twist_commands(self, twist1, twist2):
    LX = twist1.linear.x == twist2.linear.x
    LY = twist1.linear.y == twist2.linear.y
    LZ = twist1.linear.z == twist2.linear.z
    AX = twist1.angular.x == twist2.angular.x
    AY = twist1.angular.y == twist2.angular.y
    AZ = twist1.angular.z == twist2.angular.z
    equal = LX and LY and LZ and AX and AY and AZ
    if not equal:
        rospy.logwarn("The current Twist is not the same as the one sent, Resending")
    return equal

def move_robot(self, twist_object):
    # We make this to avoid Topic loss, specially at the start
    current_equal_to_new = False 
    while (not(current_equal_to_new) and not(self.shutdown_detected) ):
        self.cmd_vel_pub.publish(twist_object)
        self._cmdvel_pub_rate.sleep()
        current_equal_to_new = self.compare_twist_commands(twist1=self.last_cmdvel_command, twist2=twist_object)

def clean_class(self):
    # Stop Robot
    twist_object = Twist()
    twist_object.angular.z = 0.0
    self.move_robot(twist_object)
    self.shutdown_detected = True

 def main():
  rospy.init_node('move_robot_node', anonymous=True)

movekobuki_object = MoveKobuki()
twist_object = Twist()
# Make it start turning
twist_object.angular.z = 0.5

rate = rospy.Rate(5)

ctrl_c = False

def shutdownhook():
    # Works better than the rospy.is_shut_down()
    movekobuki_object.clean_class()
    rospy.loginfo("Shutdown Time!")
    ctrl_c = True

rospy.on_shutdown(shutdownhook)

while not ctrl_c:
    movekobuki_object.move_robot(twist_object)
    rate.sleep()

if __name__ == '__main__':
    main()

And the second part Follow_line_step_hsv.py (converts image to OpenCV to find centroid)

  #!/usr/bin/env python

  import rospy
  import cv2
  import numpy as np
  from cv_bridge import CvBridge, CvBridgeError
  from geometry_msgs.msg import Twist
  from sensor_msgs.msg import Image
  from move_robot import MoveKobuki


  class LineFollower(object):

def __init__(self):

    self.bridge_object = CvBridge()
    self.image_sub = rospy.Subscriber("/camera/image_raw",Image,self.camera_callback)
    self.movekobuki_object = MoveKobuki()

def camera_callback(self,data):

    try:
        # We select bgr8 because its the OpenCV encoding by default
        cv_image = self.bridge_object.imgmsg_to_cv2(data, desired_encoding="passthrough")
    except CvBridgeError as e:
        print(e)

    # We get image dimensions and crop the parts of the image we dont need
    # Bare in mind that because its image matrix firt value is start and second value is down limit.
    # Select the limits so that it gets the line not too close, not too far and the minimum portion possible
    # To make process faster.

    height, width, channels = cv_image.shape
    descentre = 0
    rows_to_watch = 200
    crop_img = cv_image[(height)/2+descentre:(height)/2+(descentre+rows_to_watch)][1:width]

    # Convert from RGB to HSV
    hsv = cv2.cvtColor(crop_img, cv2.COLOR_BGR2HSV)

    # Threshold the HSV image to get only ...