class DemoCar(object):
"""docstring for DemoCar"""
def __init__(self):
super(DemoCar, self).__init__()
self.image = None
self.active = True
self.rector = ImageRector()
self.car = CarDriver()
self.captured = 0
self.processed = 0
self.speed = 0.8
self.random_dir = -1
# thread.start_new_thread(camera_node.startCaptureCompressed, ())
def start_capture(self, rate=30):
self.capture = cv2.VideoCapture(0)
self.capture.set(cv2.CAP_PROP_FPS, 30)
self.last_update = time.time()
print('capture start')
self.capture_time = time.time()
while self.active:
ret, self.image = self.capture.read()
end = time.time()
self.captured = self.captured + 1
self.last_update = end
if self.captured % 150 == 0:
# print('captured %d frame in %.2f s' % (150,(time.time() - self.capture_time)))
self.capture_time = time.time()
# print('capture last update: %.2f ms' % ((end-self.last_update)*1000))
time.sleep(0.004)
print('capture end')
self.capture.release()
def follow_yellow(self):
lost_count = 0
start = time.time()
while self.image is None:
print('capture not start')
time.sleep(1)
# 根据阈值找到对应颜色
last_update = self.last_update
self.process_time = time.time()
while True:
if self.last_update == last_update:
print('image not update , skip')
time.sleep(0.005)
continue
# print('run last update %.2f ms' % (time.time() - self.last_update))
self.processed = self.processed + 1
if self.processed % 150 == 0:
print('processed %d frame in %.2f s' %
(150, (time.time() - self.process_time)))
self.process_time = time.time()
image = self.image
image = self.rector.rect(image)
image = cv2.resize(image, (320, 240))
image_raw = cv2.flip(image, -1)
image = image_raw[120:140, :]
# cnt_r = detect_cnt(image,[])
cnt_y = detect_cnt(image, yellow_hsv)
if cnt_y is not None:
# print('yellow')
# y_center,y_wh,y_angle = cv2.minAreaRect(cnt_y)
# print(y_center,y_wh,y_angle)
x, y, w, h = cv2.boundingRect(cnt_y)
y_center = [x + w / 2, y + h / 2]
cv2.drawContours(image, [cnt_y],
-1, (0, 255, 255),
thickness=2)
offset = 70 - y_center[0]
bias = offset / 160.0
self.speed = 1.0 * (1 - bias)
if self.speed < 0.8:
self.speed = 0.8
left = self.speed * (1 - bias)
right = self.speed * (1 + bias)
self.car.setWheelsSpeed(left, right)
lost_count = 0
else:
bias = 0.05 * self.random_dir
left = self.speed * (1 - bias) * 0.5
right = self.speed * (1 + bias) * 0.5
# self.car.setWheelsSpeed(left,right)
self.car.setWheelsSpeed(0, 0)
self.random_dir = self.random_dir * -1
# self.adjust_self()
lost_count = lost_count + 1
if lost_count > 15:
print('yellow line interrupt! at red line')
break
image_raw[120:140, :] = image[:, :]
cv2.imshow("images", image)
# cv2.imshow("images", image_raw)
# cv2.imshow("images", np.hstack([image, output]))
c = cv2.waitKey(2)
if c == 27:
break
def go_ahead_2_seconds(self):
self.car.setWheelsSpeed(0.9, 0.9)
time.sleep(2)
self.car.setWheelsSpeed(0, 0)
def go_ahead_and_stop(self):
self.car.setWheelsSpeed(0.9, 0.9)
time.sleep(1.8)
self.car.setWheelsSpeed(0, 0)
def go_ahead_and_detect_yellow(self):
cnt_y = None
self.car.setWheelsSpeed(0.6, 0.6)
while cnt_y is None:
image = self.image
image = self.rector.rect(image)
image = cv2.resize(image, (320, 240))
image_raw = cv2.flip(image, -1)
image = image_raw[120:140, :]
# cnt_r = detect_cnt(image,[])
cnt_y = detect_cnt(image, yellow_hsv)
self.car.setWheelsSpeed(0.5, 0.5)
self.car.setWheelsSpeed(0.3, 0.3)
def turn_left(self):
self.car.setWheelsSpeed(0.8, 0.8)
time.sleep(2)
self.car.setWheelsSpeed(0, 0.8)
time.sleep(1)
self.car.setWheelsSpeed(0.8, 0.8)
time.sleep(1)
car.setWheelsSpeed(0, 0)
def turn_right(self):
# self.car.setWheelsSpeed(0.5,0.5)
# time.sleep(0.5)
self.car.setWheelsSpeed(0.8, 0.3)
time.sleep(1.5)
car.setWheelsSpeed(0, 0)
def adjust_self(self):
image = self.image
image = cv2.resize(image, (320, 240))
cnt_r = detect_cnt(image, yellow_hsv)
if cnt_r is not None:
print('red')
r_center, r_wh, r_angle = cv2.minAreaRect(cnt_r)
print(r_center, r_wh, r_angle)
class WheelsDriverNode(object):
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing " %(self.node_name))
self.estop=False
# Parameters for maximal turning radius
self.use_rad_lim = self.setupParam("~use_rad_lim", False)
self.min_rad = self.setupParam("~min_rad", 0.08)
self.wheel_distance = self.setupParam("~wheel_distance", 0.103)
# Setup publishers
# self.driver = DaguWheelsDriver()
self.driver = CarDriver()
#add publisher for wheels command wih execution time
self.msg_wheels_cmd = WheelsCmdStamped()
self.pub_wheels_cmd = rospy.Publisher("~wheels_cmd_executed",WheelsCmdStamped, queue_size=1)
# Setup subscribers
self.control_constant = 1.0
self.sub_topic = rospy.Subscriber("~wheels_cmd", WheelsCmdStamped, self.cbWheelsCmd, queue_size=1)
self.sub_e_stop = rospy.Subscriber("~emergency_stop", BoolStamped, self.cbEStop, queue_size=1)
self.sub_rad_lim = rospy.Subscriber("~radius_limit", BoolStamped, self.cbRadLimit, queue_size=1)
self.params_update = rospy.Timer(rospy.Duration.from_sec(1.0), self.updateParams)
def setupParam(self,param_name,default_value):
value = rospy.get_param(param_name,default_value)
rospy.set_param(param_name,value) #Write to parameter server for transparancy
rospy.loginfo("[%s] %s = %s " %(self.node_name,param_name,value))
return value
def updateParams(self,event):
self.use_rad_lim = rospy.get_param("~use_rad_lim")
self.min_rad = rospy.get_param("~min_rad")
self.wheel_distance = rospy.get_param("~wheel_distance")
def cbWheelsCmd(self,msg):
if self.estop:
self.driver.setWheelsSpeed(left=0.0,right=0.0)
return
# Check if radius limitation is enabled
if (self.use_rad_lim and (msg.vel_left != 0 or msg.vel_right != 0)):
self.checkAndAdjustRadius(msg)
self.driver.setWheelsSpeed(left=msg.vel_left,right=msg.vel_right)
# Put the wheel commands in a message and publish
self.msg_wheels_cmd.header = msg.header
# Record the time the command was given to the wheels_driver
self.msg_wheels_cmd.header.stamp = rospy.get_rostime()
self.msg_wheels_cmd.vel_left = msg.vel_left
self.msg_wheels_cmd.vel_right = msg.vel_right
self.pub_wheels_cmd.publish(self.msg_wheels_cmd)
def cbRadLimit(self, msg):
rospy.set_param("~use_rad_lim", msg.data)
self.use_rad_lim = msg.data
def checkAndAdjustRadius(self, msg):
didAdjustment = False
# if both motor cmds do not have the same sign, we're demanding for an on-point turn (not allowed)
if (np.sign(msg.vel_left) != np.sign(msg.vel_right)):
# Simply set the smaller velocity to zero
if (abs(msg.vel_left) < abs(msg.vel_right)):
msg.vel_left = 0.0
else:
msg.vel_right = 0.0
didAdjustment = True
# set v1, v2 from msg velocities such that v2 > v1
if (abs(msg.vel_right) > abs(msg.vel_left)):
v1 = msg.vel_left
v2 = msg.vel_right
else:
v1 = msg.vel_right
v2 = msg.vel_left
# Check if a smaller radius than allowed is demanded
if (v1 == 0 or abs(v2 / v1) > (self.min_rad + self.wheel_distance/2.0)/(self.min_rad - self.wheel_distance/2.0)):
# adjust velocities evenly such that condition is fulfilled
delta_v = (v2-v1)/2 - self.wheel_distance/(4*self.min_rad)*(v1+v2)
v1 += delta_v
v2 -= delta_v
didAdjustment = True
# set msg velocities from v1, v2 with the same mapping as when we set v1, v2
if (abs(msg.vel_right) > abs(msg.vel_left)):
msg.vel_left = v1
msg.vel_right = v2
else:
msg.vel_left = v2
msg.vel_right = v1
return didAdjustment
def cbEStop(self,msg):
self.estop=not self.estop
if self.estop:
rospy.loginfo("[%s] Emergency Stop Activated")
else:
rospy.loginfo("[%s] Emergency Stop Released")
def on_shutdown(self):
self.driver.setWheelsSpeed(left=0.0,right=0.0)
rospy.loginfo("[%s] Shutting down."%(rospy.get_name()))
class LaneDetectorNode:
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" % (self.node_name))
self.bridge = CvBridge()
# Thread lock
self.thread_lock = threading.Lock()
self.active = True
self.verbose = True
self.car = CarDriver()
self.white_lane_his = []
self.yellow_lane_his = []
self.red_lane_his = []
self.avg_num = 5
# self.stats = Stats()
# Only be verbose every 10 cycles
self.intermittent_interval = 5
self.intermittent_counter = 0
fstream = file(rospkg.RosPack().get_path('line_detector')+'/include/line_detector/config/default.yaml', 'r')
configuration = yaml.load(fstream)
self.detector = LineDetectorHSV(configuration['detector'][1]['configuration'])
self.detector_used = self.detector
self.image_size = [120,160]
self.top_cutoff = 40
self.sub_rect = rospy.Subscriber("/camera_node/image_rect", Image, self.cbImage, queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines",Image,queue_size=1)
self.pub_edge = rospy.Publisher("~image_with_edge",Image,queue_size=1)
self.pub_colorSegment = rospy.Publisher("~image_color_segment",Image,queue_size=1)
rospy.loginfo("[%s] Initialized." % (self.node_name))
def cbImage(self,image_msg):
# self.sub_rect.unregister()
image_cv = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding="bgr8")
hei_original, wid_original = image_cv.shape[0:2]
if self.image_size[0] != hei_original or self.image_size[1] != wid_original:
# image_cv = cv2.GaussianBlur(image_cv, (5,5), 2)
image_cv = cv2.resize(image_cv, (self.image_size[1], self.image_size[0]),
interpolation=cv2.INTER_NEAREST)
image_cv = image_cv[self.top_cutoff:,:,:]
# Set the image to be detected
self.detector_used.setImage(image_cv)
# Detect lines and normals
white = self.detector_used.detectLines('white')
yellow = self.detector_used.detectLines('yellow')
red = self.detector_used.detectLines('red')
# print(white.lines)
white_c = len(white.lines)
yellow_c = len(yellow.lines)
red_c = len(red.lines)
# white_lane = np.zeros(3)
# yellow_lane = np.zeros(3)
# red_lane = np.zeros(3)
white_lane = [0,0,0]
yellow_lane = [0,0,0]
red_lane = [0,0,0]
if white_c > 0:
white_lane = self.detectLane2NoWeights(white)
if yellow_c > 0 :
yellow_lane = self.detectLane2NoWeights(yellow)
if red_c > 0 :
red_lane = self.detectLane2NoWeights(red)
self.intermittent_counter += 1
# self.drive(white_lane,yellow_lane,red_lane)
print('white d:%6f phi:%6f n:%3d , yellow d:%6f phi:%6f n:%3d , red d:%6f phi:%6f n:%3d ' % (white_lane[0],white_lane[1],
white_c,yellow_lane[0],yellow_lane[1], yellow_c, red_lane[0],red_lane[1] , red_c))
if self.verbose:
# print('line_detect_node: verbose is on!')
# Draw lines and normals
image_with_lines = np.copy(image_cv)
drawLines(image_with_lines, white.lines, (0, 0, 0))
drawLines(image_with_lines, yellow.lines, (255, 0, 0))
drawLines(image_with_lines, red.lines, (0, 255, 0))
# Publish the frame with lines
image_msg_out = self.bridge.cv2_to_imgmsg(image_with_lines, "bgr8")
image_msg_out.header.stamp = image_msg.header.stamp
self.pub_image.publish(image_msg_out)
colorSegment = color_segment(white.area, red.area, yellow.area)
edge_msg_out = self.bridge.cv2_to_imgmsg(self.detector_used.edges, "mono8")
colorSegment_msg_out = self.bridge.cv2_to_imgmsg(colorSegment, "bgr8")
self.pub_edge.publish(edge_msg_out)
self.pub_colorSegment.publish(colorSegment_msg_out)
def onShutdown(self):
rospy.loginfo("[%s] Shutdown." % (self.node_name))
def detectLane2(self,detections):
lines = np.array(detections.lines)*1.0
n_lines = len(lines)
points = np.zeros((n_lines*2,3))
for i,line in enumerate(lines):
x1,y1,x2,y2 = line
vector = np.array([ x2 - x1, y2-y1] )
weights = np.linalg.norm(vector)
points[i*2] = x1,y1,weights
points[i*2+1] = x2,y2,weights
weights_total = points[:,2].sum()
# print(lines)
# print(points)
x_m = (points[:,0]*points[:,2]).sum()/weights_total
y_m = (points[:,1]*points[:,2]).sum()/weights_total
m = ((points[:,0] - x_m)*(points[:,1]-y_m)*points[:,2]).sum()
n = ((points[:,0] - x_m)*(points[:,0] - x_m)*points[:,2]).sum()
a = m/n
b = y_m - a * x_m
phi = np.arctan(a)/np.pi*360
d1 = (80*a-80+b)/np.linalg.norm(np.array([a,-1])) #画面底部中点到直线的距离
if a == 0:
d2 = 9999
else:
d2 = (80-b)/a -80 #直线与画面底部(y=80)的交点横坐标
return d2 , phi , n_lines
def detectLane2NoWeights(self,detections):
lines = np.array(detections.lines)*1.0
n_lines = len(lines)
points = np.zeros((n_lines*2,3))
for i,line in enumerate(lines):
x1,y1,x2,y2 = line
vector = np.array([ x2 - x1, y2-y1] )
weights = np.linalg.norm(vector)
points[i*2] = x1,y1,weights/2
points[i*2+1] = x2,y2,weights/2
weights_total = points[:,2].sum()
x_m = points[:,0].mean()
y_m = points[:,1].mean()
# print("x_m:%f y_m:%f" % (x_m,y_m))
m = ((points[:,0] - x_m)*(points[:,1]-y_m)).sum()
n = ((points[:,0] - x_m)*(points[:,0] - x_m)).sum()
a = m/n
b = y_m - a * x_m
phi = np.arctan(a)/np.pi*360
d1 = (80*a-80+b)/np.linalg.norm(np.array([a,-1])) #画面底部中点到直线的距离
if a == 0:
d2 = 9999
else:
d2 = (80-b)/a - 80 #直线与画面底部(y=80)的交点横坐标距中心的距离
return d2 , phi , n_lines
# return b , a , n_lines
def drive(self,white_lane,yellow_lane,red_lane):
# return
if white_lane[2] > 10 and yellow_lane[2] > 10:
self.car.setWheelsSpeed(0.5,0.5)
elif white_lane[2] > 10 and yellow_lane[2] < 10:
self.car.setWheelsSpeed(0.4,0.7)
elif white_lane[2] < 10 and yellow_lane[2] > 10:
self.car.setWheelsSpeed(0.7,0.4)
return
white_lane_mean = [0,0,0]
yellow_lane_mean = [0,0,0]
red_lane_mean = [0,0,0]
if white_lane[2]>0:
self.white_lane_his.append(white_lane)
if len(self.white_lane_his)>=self.avg_num:
self.white_lane_his = self.white_lane_his[-self.avg_num:]
white_lane_mean = np.mean(np.array(self.white_lane_his),0)
else:
self.white_lane_his = []
if yellow_lane[2]>0:
self.yellow_lane_his.append(yellow_lane)
if len(self.yellow_lane_his)>=self.avg_num:
self.yellow_lane_his = self.yellow_lane_his[-self.avg_num:]
yellow_lane_mean = np.mean(np.array(self.yellow_lane_his),0)
else:
self.yellow_lane_mean = []
if red_lane[2]>0:
self.red_lane_his.append(red_lane)
if len(self.red_lane_his)>=self.avg_num:
self.red_lane_his = self.red_lane_his[-self.avg_num:]
red_lane_mean = np.mean(np.array(self.red_lane_his),0)
else:
self.red_lane_mean = []