class LineDetectorNode(object):
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" % (self.node_name))
self.bridge = CvBridge()
self.cv_image = None
self.detector = LineDetector()
self.visualization = True
self.size = (320, 240)
self.pid_enabled = False
self.base_speed = 50
# self.car = CarDriver()
# self.config_file = os.path.dirname(os.path.abspath(__file__)) + "/default.yaml"
self.image_msg = None
self.lost_count = 0
self.line_msg = GetLineDetectionResponse()
self.pub_detections = rospy.Publisher("~image_color",
CompressedImage,
queue_size=1)
self.pub_line_detection = rospy.Publisher("~line_detection",
LineDetection,
queue_size=1)
self.detect_line_srv = rospy.Service('~detect_line', GetLineDetection,
self.cbGetLineDetection)
self.set_color_srv = rospy.Service('~set_color_threshold',
SetColorThreshold,
self.cbSetColorThreshold)
self.get_color_srv = rospy.Service('~get_color_threshold',
GetColorThreshold,
self.cbGetColorThreshold)
self.set_color_list_srv = rospy.Service('~get_color_list', GetStrings,
self.cbGetColorList)
# self.pid_set_enable_srv = rospy.Service('~pid_set_enable', SetInt32, self.cbPidSetEnable)
# self.sub_image = rospy.Subscriber("~image_raw", Image, self.cbImg ,queue_size=1)
self.sub_image = rospy.Subscriber("~image_raw/compressed",
CompressedImage,
self.cbImg,
queue_size=1)
# rospy.loginfo("[%s] wait_for_service : camera_get_frame..." % (self.node_name))
# rospy.wait_for_service('~camera_get_frame')
# self.get_frame = rospy.ServiceProxy('~camera_get_frame', GetFrame)
rospy.loginfo("[%s] Initialized." % (self.node_name))
def cbImg(self, image_msg):
self.image_msg = image_msg
# self.line_msg = self.detectLine()
if self.pid_enabled:
self.cbPid(self.line_msg)
# self.pub_line_detection.publish(self.line_msg)
'''
def cbPid(self,line_msg):
center = line_msg.center
if center[0] == 0:
self.lost_count = self.lost_count + 1
print('lost count %d' % (self.lost_count))
if self.lost_count > 25:
self.pid_enabled = False
print('yellow line lost,stop')
self.car.setWheelsSpeed(0,0)
return
self.lost_count = 0
offset = 70 - center[0]
bias = offset/160.0
speed = self.base_speed/100.0*(1-bias)
if speed < 0.4:
speed = 0.4
left = speed*(1-bias)
right = speed*(1+bias)
self.car.setWheelsSpeed(left,right)
'''
def detectLine(self, params):
# start = time.time()
# print(params)
color = params.color.decode('utf-8')
if not self.detector.colors.has_key(color):
return GetLineDetectionResponse()
image_msg = self.image_msg
if image_msg == None:
print('[%s]: camera msg not received' % (self.node_name))
return GetLineDetectionResponse()
if hasattr(image_msg, 'format'): # CompressedImage
try:
cv_image = bgr_from_jpg(image_msg.data)
except ValueError as e:
rospy.loginfo('[%s] cannot decode image: %s' %
(self.node_name, e))
return GetLineDetectionResponse()
else: # Image
try:
cv_image = self.bridge.imgmsg_to_cv2(image_msg,
desired_encoding="bgr8")
except Exception as e:
rospy.loginfo('[%s] cannot convert image: %s' %
(self.node_name, e))
return GetLineDetectionResponse()
# if cv_image.shape[0]!=self.size[1] or cv_image.shape[1]!=self.size[0]:
# cv_image = cv2.resize(cv_image,self.size)
if params.y1 < params.y2 and params.x1 < params.x2:
rect_image = cv_image[params.y1:params.y2, params.x1:params.x2]
detection, image = self.detector.detect_hsv(
rect_image, self.detector.colors[color])
if self.visualization:
cv_image[params.y1:params.y2, params.x1:params.x2] = image
cv2.rectangle(cv_image, (params.x1, params.y1),
(params.x2, params.y2), (0, 0, 0), 1)
# cv_image = cv2.resize(cv_image,(480,360))
# imgmsg = self.bridge.cv2_to_imgmsg(cv_image,"bgr8")
# self.pub_detections.publish(imgmsg)
self.pubImage(cv_image)
end = time.time()
# print('time cost in detect line: %.2f ms' % ((end - start)*1000))
return GetLineDetectionResponse(detection[0:2], detection[2:4],
detection[4])
def toLineDetections(self, cnt):
if cnt is not None:
center, wh, angle = cv2.minAreaRect(cnt)
return GetLineDetectionResponse(center, wh, angle)
return GetLineDetectionResponse()
def cbGetLineDetection(self, params):
try:
line_msg = self.detectLine(params)
return line_msg
except Exception as e:
print(self.node_name)
print(e)
return GetLineDetectionResponse()
# return self.line_msg
def cbSetColorThreshold(self, params):
print(params)
try:
self.detector.colors[params.color.decode('utf-8')] = [{
"min": [params.low_h, params.low_s, params.low_v],
"max": [params.high_h, params.high_s, params.high_v]
}]
return SetColorThresholdResponse("设置成功")
except Exception as e:
print(self.node_name)
print(e)
return SetColorThresholdResponse("设置失败,请检查参数")
def cbGetColorThreshold(self, params):
try:
threshold = self.detector.colors[params.color.decode('utf-8')]
return GetColorThresholdResponse(str(threshold))
except Exception as e:
print(self.node_name)
print(e)
return GetColorThresholdResponse()
def cbGetColorList(self, params):
return GetStringsResponse(self.detector.colors.keys())
'''
def cbPidSetEnable(self,params):
if params.port == 1:
self.pid_enabled = True
self.base_speed = params.value
else:
self.pid_enabled = True
self.base_speed = 0
self.car.setWheelsSpeed(0,0)
return SetInt32Response(params.port,params.value)
'''
def pubImage(self, image):
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = jpg_from_bgr(image)
self.pub_detections.publish(msg)
def onShutdown(self):
rospy.loginfo("[%s] Shutdown." % (self.node_name))
import cv2
import time
from line_detector import LineDetector
detector = LineDetector()
subscriber = RosImageSubscriber()
count = 1
start = time.time()
while True:
# image_frame = subscriber.get_frame(GetFrameRequest())
# cv_image = subscriber.bridge.imgmsg_to_cv2(image_frame.image, desired_encoding="bgr8")
cv_image = subscriber.cv_image
rect_image = cv_image
detection, image_color = detector.detect_hsv(rect_image,
detector.colors[u'黄色'])
count = count + 1
if (count > 100):
end = time.time()
print('detect %d frame in %.2f seconds, avarage %.1f ms/frame' %
(count, (end - start), (end - start) / count * 1000))
count = 0
start = time.time()
# if detection[0] != 0:
# print(detection)
continue
cv2.imshow('image', cv_image)
c = cv2.waitKey(2)
if c == 27:
break
