|
@@ -1,4 +1,13 @@
|
|
|
#!/usr/bin/env python
|
|
|
+'''
|
|
|
+Author: adam_zhuo
|
|
|
+Date: 2020-10-21 16:31:05
|
|
|
+LastEditTime: 2021-01-13 16:06:33
|
|
|
+LastEditors: Please set LastEditors
|
|
|
+Description: In User Settings Edit
|
|
|
+FilePath: /blackTornadoRobot/src/rasp_camera/scripts/line_follow.py
|
|
|
+'''
|
|
|
+
|
|
|
import rospy, cv2, cv_bridge, numpy, math, datetime
|
|
|
from sensor_msgs.msg import Image
|
|
|
from geometry_msgs.msg import Twist
|
|
@@ -33,7 +42,7 @@ class Follower:
|
|
|
|
|
|
# ROI
|
|
|
# cropped = img[y0:y1, x0:x1]
|
|
|
- cropped = img[190:240, 20:300]
|
|
|
+ cropped = img[0:140, 20:300]
|
|
|
|
|
|
cv2.imshow('window_cropped',cropped)
|
|
|
cv2.waitKey(3)
|
|
@@ -53,7 +62,7 @@ class Follower:
|
|
|
y, x = (binimg < 125).nonzero()
|
|
|
|
|
|
# Judge center of line
|
|
|
- x0 = 160
|
|
|
+ x0 = 140
|
|
|
if x.shape[0] != 0:
|
|
|
y0 = (max(y) + min(y))/2
|
|
|
x0 = (max(x) + min(x))/2
|
|
@@ -69,7 +78,7 @@ class Follower:
|
|
|
else:
|
|
|
self.cnt = 0
|
|
|
# control
|
|
|
- err = x0 - 160
|
|
|
+ err = x0 - 140
|
|
|
print 'err:', err
|
|
|
self.twist.linear.x = 0.08
|
|
|
self.twist.angular.z = 0.0
|