limo_ros1_api/lane_detect.py at master · WeGo-Robotics/limo_ros1_api · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
from pymap3d import spherical2geodetic
import rospy
from sensor_msgs.msg import CompressedImage
from geometry_msgs.msg import Twist
from cv_bridge import CvBridge
import cv2
import numpy as np
from math import *

class Sub_class:
   def __init__(self):
      rospy.init_node("wego_sub_node")
      rospy.Subscriber("/camera/rgb/image_raw/compressed",CompressedImage,self.camera_cb)
      self.cmd_pub=rospy.Publisher("/cmd_vel",Twist,queue_size=1)
      self.bridge = CvBridge()
      self.cmd_msg = Twist()
      self.standard = 80
   def camera_cb(self,msg):
      cv_img = self.bridge.compressed_imgmsg_to_cv2(msg)
      y,x, channel = cv_img.shape
      hsv_img = cv2.cvtColor(cv_img,cv2.COLOR_BGR2HSV)
      lower = np.array([15,60,0])
      upper = np.array([45,255,255])

      filtered_img = cv2.inRange(hsv_img,lower,upper)
      margin_x = 240
      margin_y = 280
      src1=[0,y]
      src2=[x,y]
      src3=[margin_x,margin_y]
      src4=[x-margin_x,margin_y]
      srcs=np.float32([src1,src2,src3,src4])
      dst1=[80,y]
      dst2=[x-80,y]
      dst3=[80,0]
      dst4=[x-80,0]
      dsts=np.float32([dst1,dst2,dst3,dst4])
      matrix = cv2.getPerspectiveTransform(srcs,dsts)
      warp_img = cv2.warpPerspective(filtered_img,matrix,[x,y])

      bin_img = np.zeros_like(warp_img)
      bin_img[0<warp_img] = 1
      bin_img=bin_img[:,0:x//2]
      ver_hist = np.sum(bin_img,axis=0)
      cv2.imshow("bin_img",bin_img)
      ver_hist[ver_hist<20]=0

      ver_nz_indices = np.nonzero(ver_hist)[0]

      # print(ver_hist)
      try:
         ver_avg_index = (ver_nz_indices[0]+ver_nz_indices[-1]) //2
         cv2.line(cv_img,[ver_avg_index,0],[ver_avg_index,y],[0,0,255],5)

      except:
         ver_avg_index=self.standard
      cv2.line(cv_img,[self.standard,0],[self.standard,y],[0,255,0],5)
      diff_pixel = self.standard - ver_avg_index
      self.cmd_msg.linear.x = 0.15
      self.cmd_msg.angular.z = diff_pixel*pi/x
      self.cmd_pub.publish(self.cmd_msg)
      cv2.imshow("filtered_img",filtered_img)
      cv2.imshow("warp_img",warp_img)
      cv2.imshow("cv_img",cv_img)

      cv2.waitKey(1)

sub_class = Sub_class()
rospy.spin()