line_tracing

#!/usr/bin/env python
import rospy,cv2,math
import numpy as np
from sensor_msgs.msg import Image, LaserScan
from std_msgs.msg import Int32
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import Twist

CENTER_X = 200
DETECT_Y = 230  #center = 150
DETECT_LINE_LENGTH = 330
SENSITIVITY = 60

#hsv
yellow_lower = [22,60-SENSITIVITY,200-SENSITIVITY]
yellow_upper = [60,255,255]
white_lower = [0,0,255-SENSITIVITY]
white_upper = [179,SENSITIVITY,255]

bridge = CvBridge()
velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

def detectLine(img):
  for x_axis in range(CENTER_X - DETECT_LINE_LENGTH/2,CENTER_X + DETECT_LINE_LENGTH/2):
    # img rank is 3 and img[DETECT_Y,x_axis] rank is one, so apply [[ ]] to img[DETECT_Y,x_axis], and make its rank 3
    color = np.uint8([[img[DETECT_Y,x_axis]]])
    # opencv hsv range h:0-179, s:0-255, v:0-255
    # bgr -> hsv
    color = cv2.cvtColor(color,cv2.COLOR_BGR2HSV)[0][0]
    if((color[0] >= yellow_lower[0] and color[0] <= yellow_upper[0]) and (color[1] >= yellow_lower[1] and color[1] <= yellow_upper[1]) and (color[2] >= yellow_lower[2] and color[2] <= yellow_upper[2])):
      if(x_axis>CENTER_X):return "Left"
      else:return "Right"
    elif((color[0] >= white_lower[0] and color[0] <= white_upper[0]) and (color[1] >= white_lower[1] and color[1] <= white_upper[1]) and (color[2] >= white_lower[2] and color[2] <= white_upper[2])):
      if(x_axis>CENTER_X):return "Left"
      else:return "Right"
  return "Straight"

# Don't need this method in simple line tracing.
def detectLIDAR(scan):
  for i in range(0,len(scan.ranges)-1):
    angle = scan.angle_min + i*scan.angle_increment
    dx=scan.ranges[i]*math.cos(angle)
    dy=scan.ranges[i]*math.sin(angle)
    if (scan.ranges[i]>0.05 and dx<0.30 and dx>0):
      if(dy>-0.15 and dy<0): return "Left"
      if(dy<0.15 and dy>0):  return "Right"
  return "Straight"

def moveTurtlebot(move):
  print(move)
  # generate Twist: angular velocity, lineasr velocity
  vel_msg = Twist()
  if (move=="Left"):    vel_msg.angular.z = 0.1
  elif (move=="Right"): vel_msg.angular.z = -0.1
  else:                 vel_msg.linear.x = 0.1
  velocity_publisher.publish(vel_msg)

def imageCallback(data):
  img = bridge.imgmsg_to_cv2(data, "bgr8")
  move_command = detectLine(img)
  moveTurtlebot(move_command)
  # maybe it is showing about detect line to screen
  # cv2.line(img, start, end, color, thickness) - thickness: line thickness.pixel
  test_line = cv2.line(img,(CENTER_X - DETECT_LINE_LENGTH/2,DETECT_Y),(CENTER_X + DETECT_LINE_LENGTH/2,DETECT_Y),(255,0,0),3)
  cv2.imshow("DetectLine",test_line)
  # waitKey(0) will display the window infinitely until any keypress (it is suitable for image display).
  # waitKey(1) will display a frame for 1 ms, after which display will be automatically closed
  # waitKey(1) is proper in this situation, because this program need new image insteadily
  # so there have to be no time delay
  # without below line this program works, but n is too big in waitKey(n), the robot can't change its velocity instantly
  cv2.waitKey(1)

def lidarCallback(data):
  move_command = detectLIDAR(data)
  moveTurtlebot(move_command)

def listener():
  rospy.init_node('line_detector', anonymous=True)
  rospy.Subscriber("/image_raw", Image, imageCallback)
  # rospy.Subscriber("/scan", LaserScan, lidarCallback)
  rospy.spin()

if __name__ == '__main__':
    listener()