def gen_blog_entry(self, roi_id, obj_id, objs):
print 'Region: ' + self.get_roi_name(roi_id)
time = dt.fromtimestamp(int(rospy.get_time()))
body = '### CHANGE DETECTION REPORT\n\n'
body += '- **Region:** ' + self.get_roi_name(roi_id) + '\n\n'
#body += '- **Object ID:** ' + str(obj_id) + '\n\n'
body += '- **Time:** ' + str(time) + '\n\n'
# Create some blog entries
msg_store = MessageStoreProxy(collection=self.blog_collection)
robblog_path = roslib.packages.get_pkg_dir('soma_utils')
for idx,obj in enumerate(objs):
bridge = CvBridge()
im = bridge.imgmsg_to_cv2(obj.image_mask, desired_encoding="bgr8")
imgray = cv2.cvtColor(im,cv2.COLOR_BGR2GRAY)
ret,thresh = cv2.threshold(imgray,1,1,1) #cv2.threshold(imgray,127,255,0)
contours, hierarchy = cv2.findContours(thresh,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
bridge = CvBridge()
cv_image = bridge.imgmsg_to_cv2(obj.image_full, desired_encoding="bgr8")
cv2.drawContours(cv_image,contours,-1,(0,0,255),2)
full_scene_contour = bridge.cv2_to_imgmsg(cv_image)
img_id = msg_store.insert(full_scene_contour)
body += '<font color="red">Detected change (' + str(idx+1) + '/'+ str(len(objs)) + '):</font>\n\n)\n\n' % img_id
e = RobblogEntry(title=str(time) + " Change Detection - " + self.roi_name[roi_id], body= body )
msg_store.insert(e)
class ROSSubscriber(object):
def __init__(self, parent, data, loglevel):
self._parent = parent
self._data = data
self.logger = logging.getLogger('ROSSubscriber')
self.logger.addHandler(CH)
self.logger.setLevel(loglevel)
self.hand_sub = mfilters.Subscriber(
"hand", Image)
self.skel_sub = mfilters.Subscriber(
"skeleton", Image)
self.clas_sub = mfilters.Subscriber(
"class", TimeReference)
self.image_ts = mfilters.TimeSynchronizer(
[self.hand_sub, self.skel_sub,
self.clas_sub], 30)
self.image_ts.registerCallback(
self.callback)
self.bridge = CvBridge()
def callback(self, hand, skel, class_name):
hand = self.bridge.imgmsg_to_cv2(hand,
desired_encoding=
'passthrough')
skel = self.bridge.imgmsg_to_cv2(skel,
desired_encoding=
'passthrough')
self._data.add(hand, skel, class_name.source,
class_name.time_ref.secs)
evt = CreateEvent(EVT_ROS_TYPE, -1)
wx.PostEvent(self._parent, evt)
def run():
global right_image
global left_image
# Init node
rospy.init_node("stereo_vision");
# Subscribe for two "eyes"
rospy.Subscriber("/stereo/right/image_raw", Image, right_image_callback)
rospy.Subscriber("/stereo/left/image_raw", Image, left_image_callback)
stereo_publisher = rospy.Publisher("/vision/stereo", Image, queue_size=1000)
# Action
print "Start processing..."
bridge = CvBridge()
worker = StereoVision()
# rate = rospy.Rate(10)
while not rospy.is_shutdown():
if right_image != None and left_image != None:
cv_right_image = bridge.imgmsg_to_cv2(right_image, desired_encoding="bgr8")
cv_left_image = bridge.imgmsg_to_cv2(left_image, desired_encoding="bgr8")
cv_stereo_image = worker.create_stereo_image( cv_right_image, cv_left_image )
stereo_image = bridge.cv2_to_imgmsg(cv_stereo_image, encoding="bgr8")
stereo_publisher.publish( stereo_image )
class PatchSaver(object):
def __init__(self):
rospy.init_node('patch_saver',log_level=rospy.DEBUG)
self.patch_array_sub = rospy.Subscriber('patch_array', PatchArray,
self.patch_array_callback, None, 1)
self.debug_img_pub = rospy.Publisher('patch_array_debug_img', Image)
self.bridge = CvBridge()
self.path = rospy.get_param('~save_path')
def patch_array_callback(self, PatchArray):
rospy.loginfo("Patch Array Callback")
num = np.random.random()
if ((len(PatchArray.patch_array) > 1) and num > 0.9) or (num > 0.95):
for idx, patch in enumerate(PatchArray.patch_array):
image = np.asarray(self.bridge.imgmsg_to_cv2(patch.image,'bgr8'))
mask = np.asarray(self.bridge.imgmsg_to_cv2(patch.mask,'mono8'))
cv2.imwrite(self.path +
str(int(PatchArray.header.stamp.to_sec() * 1000))
+ "_" + str(idx) + "_image.png", image)
cv2.imwrite(self.path +
str(int(PatchArray.header.stamp.to_sec() * 1000))
+ "_" + str(idx) + "_mask.png", mask)
def callback(data):
global zero, prev
#cv2.namedWindow("flow",1)
#cv2.moveWindow("flow",500,00)
#cv2.namedWindow("zero_image",1)
#cv2.moveWindow("zero_image",600,0)
bridge = CvBridge()
if zero:
prev = bridge.imgmsg_to_cv2(data, "mono8")
cv_image = cv2.flip(prev,1)
#ret1,prev = cv2.threshold(prev, 0, 0,cv2.THRESH_BINARY)
zero = False
#cv2.imshow("zero_image", prev)
# cv_image = bridge.imgmsg_to_cv2(data, "bgr8")
cv_image = bridge.imgmsg_to_cv2(data, "mono8")
#gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
cv_image = cv2.flip(cv_image,1)
#cv2.imshow("motion_detector", cv_image)
motion_detector(cv_image, prev)
def callback2(data):
global face_det
global filename
global datei
#rospy.loginfo(rospy.get_caller_id()+"CDIA data:" + str(len(data.head_detections)))
if len(data.head_detections)==1 and len(data.head_detections[0].face_detections) == 1:
face_det+=1
print face_det
bridge = CvBridge()
image = bridge.imgmsg_to_cv2(data.head_detections[0].color_image,"rgb8")
depth = bridge.imgmsg_to_cv2(data.head_detections[0].depth_image,"32FC3")
#cv2.imshow("image",image)
cv2.imshow("depth",depth)
cv2.waitKey()
print depth.shape
set_name = datei[1]
depth_path="/home/stefan/rgbd_db_heads/"+set_name+"/"+filename+"_d.xml"
img_path="/home/stefan/rgbd_db_heads/"+set_name+"/"+filename+"_c.bmp"
#path+"/"+str(dir)+"/"+os.path.splitext(file)[0]+".xml"
depth_slice1 = depth[:,:,0].astype(numpy.float32)
depth_slice2 = depth[:,:,1].astype(numpy.float32)
depth_slice3 = depth[:,:,2].astype(numpy.float32)
for(r,c),value in numpy.ndenumerate(depth_slice1):
if depth_slice1[r,c]==0:
depth_slice1[r,c]=numpy.nan
for(r,c),value in numpy.ndenumerate(depth_slice2):
if depth_slice2[r,c]==0:
depth_slice2[r,c]=numpy.nan
print filename
class ButtonGui(QDialog):
def __init__(self):
QDialog.__init__(self)
self.controller = BasicDroneController()
uic.loadUi(join(dirname(__file__), 'mav_control.ui'), self)
self.setWindowTitle('AR.Drone Video Feed')
self.cv = CvBridge()
self.trackingColor = np.array([1, 0, 0], dtype=np.float32)
def videoFrame(self, image):
self.cv_image = self.cv.imgmsg_to_cv2(image, "rgb8")
self.cv_image = cv2.cvtColor(self.cv_image, cv2.COLOR_BGR2RGB)
self.cv_image = cv2.resize(self.cv_image, (self.cv_image.shape[1]/2, self.cv_image.shape[0]/2))
lab_img, cont_image, center_mass, cont_area = find_car(self.cv_image, self.trackingColor, self.hsThreshold.value()/100.0)
self.cv_image = find_rectangles(self.cv_image)
qi = QImage(cont_image.data, cont_image.shape[1], cont_image.shape[0], QImage.Format_RGB888).rgbSwapped()
self.lbVideo.setFixedHeight(cont_image.shape[0])
self.lbVideo.setFixedWidth(cont_image.shape[1])
self.lbVideo.setPixmap(QPixmap.fromImage(qi))
x_center = center_mass[0]
y_center = center_mass[1]
self.handle(cont_area)
if self.cbAuto.isChecked():
self.fly(x_center, y_center, cont_area)
else:
self.lbAuto.setText('Disabled.')
def handle(self, cont_area):
self.area = cont_area
def videoFrame1(self, image):
self.cv_image = self.cv.imgmsg_to_cv2(image, "rgb8")
self.cv_image = cv2.cvtColor(self.cv_image, cv2.COLOR_BGR2RGB)
self.cv_image = cv2.resize(self.cv_image, (self.cv_image.shape[1]/2, self.cv_image.shape[0]/2))
lab_img, cont_image, center_mass, cont_area = find_car(self.cv_image, self.trackingColor, self.hsThreshold.value()/100.0)
self.cv_image = find_rectangles(self.cv_image)
qi = QImage(cont_image.data, cont_image.shape[1], cont_image.shape[0], QImage.Format_RGB888).rgbSwapped()
self.lbVideo1.setFixedHeight(cont_image.shape[0])
self.lbVideo1.setFixedWidth(cont_image.shape[1])
self.lbVideo1.setPixmap(QPixmap.fromImage(qi))
def fly(self, x_center, y_center, cont_area):
pass
# On a mouse press, select a tracking color.
def mousePressEvent(self, QMouseEvent):
x = QMouseEvent.x() - self.lbVideo.x()
y = QMouseEvent.y() - self.lbVideo.y()
if x >= 0 and y >= 0 and x < self.lbVideo.width() and y < self.lbVideo.height():
self.trackingColor = np.array(self.cv_image[y, x], dtype=np.float32)/255.0
class pathfinder:
cv_image = None
cv_depth_image = None
thresh = None
def __init__(self):
self.bridge = CvBridge()
cam_data = rospy.Subscriber("camera_data", CameraData, self.rgb_filter)
def rgb_filter(self, data):
cv_image = self.bridge.imgmsg_to_cv2(data.rgb_image)
cv_image = cv2.flip(cv_image, 0)
depth_thresh = self.bridge.imgmsg_to_cv2(data.thresh_image)
#depth_thresh = cv2.resize(depth_thresh, (640,480), interpolation=cv2.INTER_AREA)
cv_image_mask = cv2.bitwise_and(cv_image, cv_image, mask=depth_thresh)
grayscale_thresh = self.gray_filter(cv_image_mask)
ret, contours, hierarchy = cv2.findContours(grayscale_thresh,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
self.cnt = self.contour_filter(contours)
img = cv2.drawContours(cv_image, self.cnt, -1, (0,255,0), 3)
cv2.fillPoly(cv_image, pts =[self.cnt], color=(0,255,0))
cv2.imshow("Image Window", cv_image)
#cv2.imshow("Grayscale", grayscale_thresh)
cv2.waitKey(3)
def hsv_filter(self, cv_image):
hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
mask = np.zeros(cv_image.shape[:2], np.uint8)
mask[200:500, 200:400] = 255
masked_img = cv2.bitwise_and(cv_image,cv_image,mask = mask)
#hist_mask = cv2.calcHist([cv_image],[0],mask,[256],[0,256])
mean_hsv = cv2.mean(hsv, mask = mask)
lower_hsv = [self.calc_hsv(mean_hsv[0],1), self.calc_hsv(mean_hsv[1],1), self.calc_hsv(mean_hsv[2],1)]
upper_hsv = [self.calc_hsv(mean_hsv[0],0), self.calc_hsv(mean_hsv[1],0), self.calc_hsv(mean_hsv[2],0)]
np_lower_hsv = np.array(lower_hsv, dtype=np.uint8)
np_upper_hsv = np.array(upper_hsv, dtype=np.uint8)
thresh = cv2.inRange(hsv, np_lower_hsv, np_upper_hsv)
return thresh
def gray_filter(self, cv_image):
grayscale = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
mask = np.zeros(cv_image.shape[:2], np.uint8)
mask[300:500, 150:450] = 255
masked_img = cv2.bitwise_and(cv_image,cv_image,mask = mask)
mean_gray = cv2.mean(grayscale, mask=mask)
#cv2.imshow("Mask", mask)
thresh = cv2.inRange(grayscale, mean_gray[0]-20, mean_gray[0]+20)
return thresh
def contour_filter(self, contours):
return max(contours, key=cv2.contourArea)
def calc_hsv(self, hsv, over_under):
if over_under==1:
return int(hsv-(float(hsv/2.0)))
else:
return int(hsv+(float(hsv/2.0)))
class cvBridgeDemo():
def __init__(self):
self.node_name = "cv_bridge_demo"
rospy.init_node(self.node_name)
# What we do during shutdown
rospy.on_shutdown(self.cleanup)
# Create the OpenCV display window for the RGB image
self.bridge = CvBridge()
self.clock = rospy.Time()
self.intR = 0000
# Create a depth generator
# Subscribe to the camera image and depth topics and set
# the appropriate callbacks
self.action = 'stop'
self.I = []
self.RGB_sub = message_filters.Subscriber('/camera/rgb/image_rect_color', Image)
self.Depth_sub = message_filters.Subscriber('/camera/depth_registered/hw_registered/image_rect', Image)
# rospy.Subscriber('Scene',String,callbackS)
self.ts = message_filters.ApproximateTimeSynchronizer([self.RGB_sub, self.Depth_sub], 1000,0.02)
# self.image_sub = rospy.Subscriber("/camera/rgb/image_rect_color", Image, self.image_callback)
# self.depth_sub = rospy.Subscriber("/camera/depth/image_raw", Image, self.depth_callback)
self.ts.registerCallback(self.image_callback)
cv2.namedWindow("RGB",cv2.WINDOW_AUTOSIZE)
rospy.loginfo("Waiting for image topics...")
def image_callback(self, ros_image, depth_image):
try:
depth_image = self.bridge.imgmsg_to_cv2(depth_image, "passthrough")
frame = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")
# d_image = self.bridge.imgmsg_to_cv2(depth_image, "passthrough")
except CvBridgeError, e:
print e
# start_time = timeit.default_timer() # Convert the image to a Numpy array since most cv2 functions
# require Numpy arrays.
frame = np.array(frame, dtype=np.uint8)
depth_array = np.array(depth_image, dtype=np.float32)
depth_array = np.nan_to_num(depth_array)
# gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# gray = cv2.equalizeHist(gray)
# faceCascade = cv2.CascadeClassifier("face_cascada.xml")
# faces = faceCascade.detectMultiScale(gray, scaleFactor=1.3, minNeighbors=4, minSize=(30, 30),
# flags=cv2.cv.CV_HAAR_SCALE_IMAGE)
# print depth_array.shape
# for (x, y, w, h) in faces:
# cv2.rectangle(frame,(x,y),(x+w,y+h),(255,255,0),2)
# print depth_array[y+(h/2),x+(w/2)]
if self.action == 'save':
print "Saving..."
self.I.append((frame,depth_array))
cv2.imshow("RGB",frame)
key = cv2.waitKey(10) & 0xFF
if key == ord('s'):
self.action = 'save'
elif key == ord('q'):
self.action = 'stop'
self.intR += 1
class image_converter:
def __init__(self):
rospy.init_node('image_converter', anonymous=True)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/stereo/left/image_raw",Image,self.callback_left)
self.image_sub = rospy.Subscriber("/stereo/right/image_raw",Image,self.callback_right)
self.left=[]
self.right=[]
def cropcrop(self,frame):
par = Parameters
matrix = create_distortion_matrix(
par.fxL, par.cxL, par.fyL, par.cyL
)
frame = translate(frame, par.xL + par.xo, par.yL + par.yo)
frame = transform(frame, matrix)
frame = translate(frame, par.xo2, par.yo2)
frame = crop(
frame,
par.cropXL,
par.cropXR,
par.cropYL,
par.cropYR,
)
return frame
def callback_left(self,data):
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
self.left=cv_image
def callback_right(self,data):
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
self.right=cv_image
def virtualize(self):
rate = rospy.Rate(30) # 10hz
while not rospy.is_shutdown():
if (self.left!=[] and self.right!=[]):
left_frame=self.cropcrop(cv2.flip(self.left,1))
right_frame=self.cropcrop(cv2.flip(self.right,1))
#put two images together
composite_frame = join_images(
right_frame,
left_frame,
)
composite_frame=cv2.transpose(composite_frame)
cv2.namedWindow("test", cv2.WND_PROP_FULLSCREEN)
cv2.setWindowProperty("test", cv2.WND_PROP_FULLSCREEN, cv2.cv.CV_WINDOW_FULLSCREEN)
cv2.imshow("test",composite_frame)
cv2.waitKey(3)
rate.sleep()
def cb(self, ubd, rgb, d):
b = CvBridge()
rgb = b.imgmsg_to_cv2(rgb)[:,:,::-1] # Need to do BGR-RGB conversion manually.
d = b.imgmsg_to_cv2(d)
for i, detrect in enumerate(zip(ubd.pos_x, ubd.pos_y, ubd.width, ubd.height)):
cv2.imwrite(pjoin(self.dirname, "{:06d}.png".format(self.counter)), cutout(rgb, detrect, self.hfact))
det_d = cutout(d, detrect, self.hfact)
stderr.write("\r{}".format(self.counter)) ; stderr.flush()
self.counter += 1
class cvBridgeDemo():
def __init__(self,path_base,path):
self.node_name = "cv_bridge_demo"
rospy.init_node(self.node_name)
self.path_base = path_base+"/"+path
if not os.path.exists(self.path_base+"/RGB"):
os.makedirs(self.path_base+"/RGB")
if not os.path.exists(self.path_base + "/Depth"):
os.makedirs(self.path_base + "/Depth")
# What we do during shutdown
rospy.on_shutdown(self.cleanup)
# Create the OpenCV display window for the RGB image
self.bridge = CvBridge()
self.label='None'
self.clock = rospy.Time()
self.init = int(0)
self.end = int(10000000)
self.intR = 0000
self.file = open(self.path_base+"/List.txt",'w')
# Create a depth generator
# Subscribe to the camera image and depth topics and set
# the appropriate callbacks
self.RGB_sub = message_filters.Subscriber('/camera/rgb/image_rect_color', Image)
self.Depth_sub = message_filters.Subscriber('/camera/depth_registered/hw_registered/image_rect', Image)
self.ts = message_filters.ApproximateTimeSynchronizer([self.RGB_sub, self.Depth_sub], 1000,0.02)
self.ts.registerCallback(self.image_callback)
rospy.loginfo("Waiting for image topics...")
def image_callback(self, ros_image, depth_image):
# Use cv_bridge() to convert the ROS image to OpenCV format
try:
frame = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")
depth = self.bridge.imgmsg_to_cv2(depth_image, "passthrough")
# d_image = self.bridge.imgmsg_to_cv2(depth_image, "passthrough")
except CvBridgeError, e:
print e
depth_array = np.array(depth, dtype=np.float32)
depth_array = np.nan_to_num(depth_array)
self.clock = rospy.get_time()
# Convert the image to a Numpy array since most cv2 functions
# require Numpy arrays.
cv2.imwrite(self.path_base + "/RGB" + "/Frame_" + self.intR.__str__() + "_RGB.jpg", frame)
np.save(self.path_base + "/Depth" + "/Frame_" + self.intR.__str__() + "_Depth.npy", depth_array)
self.file.write(self.clock.__str__() + "\n" +
"Frame_" + self.intR.__str__() + "_RGB.jpg" + "\n" +
"Frame_" + self.intR.__str__() + "_Depth.png\n" + self.label + "\n")
self.intR += 1
class Echo:
def __init__(self):
self.node_name = "BlobDetector"
self.thread_lock = threading.Lock()
self.sub_image = rospy.Subscriber("/camera/rgb/image_rect_color",\
Image, self.cbImage, queue_size=1)
self.pub_image = rospy.Publisher("/blob_image",\
Image, queue_size=1)
self.pub_data = rospy.Publisher("/blob_detections", BlobDetections, queue_size = 1)
self.bridge = CvBridge()
rospy.loginfo("[%s] Initialized." %(self.node_name))
def cbImage(self,image_msg):
thread = threading.Thread(target=self.processImage,args=(image_msg,))
thread.setDaemon(True)
thread.start()
def processImage(self, image_msg):
if not self.thread_lock.acquire(False):
return
image_cv = self.bridge.imgmsg_to_cv2(image_msg)
msg, img = process(image_cv)
try:
self.pub_image.publish(self.bridge.cv2_to_imgmsg(img, "bgr8"))
self.pub_data.publish(msg)
except CvBridgeError as e:
print(e)
self.thread_lock.release()
class image_subscriber:
def __init__(self, object_name):
self.object_name = object_name
self.bridge = CvBridge()
self.rospack = rospkg.RosPack()
#rospy.wait_for_service('getImage')
#self.getImage_client = rospy.ServiceProxy('image_preparator/getImage', getImage)
#self.UpdateTFs_client = rospy.ServiceProxy('image_preparator/UpdateTFs', UpdateTFs)
self.SaveCloud_client = rospy.ServiceProxy('cloud_saver/SaveCloud', SaveCloud)
prefix = self.rospack.get_path('deep_cnn_object_detection')
dir_path = prefix + '/data/images/' + self.object_name
if not os.path.exists(dir_path):
os.makedirs(dir_path)
pass
def get_image(self):
res = self.getImage_client()
data = res.msg
try:
self.image = self.bridge.imgmsg_to_cv2(data, "bgr8") # bgr or rgb it is no matter, it will be in bgr in both cases
except CvBridgeError as e:
print e
def save_image(self, image_num):
prefix = self.rospack.get_path('deep_cnn_object_detection')
image_path = prefix + '/data/images/' + self.object_name + '/' + str(image_num) + '.jpg'
cv2.imwrite(image_path, self.image)
pass
def save_cloud(self, angle):
self.SaveCloud_client(angle);
pass
class image_converter:
def __init__(self):
cv2.namedWindow("detected circles", 1)
cv2.startWindowThread()
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/turtlebot_2/camera/rgb/image_raw",
Image, self.callback)
def callback(self, data):
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
img = cvtColor(cv_image, COLOR_BGR2GRAY)
img = cv2.medianBlur(img,5)
circles = cv2.HoughCircles(img,cv.CV_HOUGH_GRADIENT,4,20,
param1=230,param2=230,minRadius=0,maxRadius=0)
if circles is not None:
print len(circles)
circles = np.uint16(np.around(circles))
for i in circles[0,:]:
# draw the outer circle
cv2.circle(cv_image,(i[0],i[1]),i[2],(0,255,0),2)
# draw the center of the circle
cv2.circle(cv_image,(i[0],i[1]),2,(0,0,255),3)
cv2.imshow('detected circles',cv_image)
class ImageGUI(object):
def __init__(self, filename, image_topic):
rospy.init_node('record_user_input')
# initialize display
self.window_name = 'user_input'
cv2.namedWindow(self.window_name,1)
self.subImage = rospy.Subscriber(image_topic, Image, self.image_callback, queue_size=5, tcp_nodelay=True)
self.cvbridge = CvBridge()
# initialize file
self.csvfile = open(filename, 'wb')
self.datawrite = csv.writer(self.csvfile, delimiter=',', quotechar='|', quoting=csv.QUOTE_MINIMAL)
# user input
cv2.setMouseCallback(self.window_name, self.on_mouse_click)
self.mouse_position = [0,0]
def image_callback(self, rosimg):
# Convert the image.
try:
img = self.cvbridge.imgmsg_to_cv2(rosimg, 'passthrough') # might need to change to bgr for color cameras
except CvBridgeError, e:
rospy.logwarn ('Exception converting background image from ROS to opencv: %s' % e)
img = np.zeros((320,240))
cv2.imshow(self.window_name, img)
ascii_key = cv2.waitKey(1)
if ascii_key != -1:
self.on_key_press(ascii_key)
def shapeCallback(req): rosimg = req.im; cvbridge = CvBridge() try: bw_img = cvbridge.imgmsg_to_cv2(rosimg, desired_encoding="passthrough") except CvBridgeError, e: print e
class ImageScale:
def __init__(self, width=160, height=120):
self.width = width
self.height = height
self.nPrints = 1
self.bridge = CvBridge()
rospy.init_node('camera_scaled', anonymous=True)
rospy.Subscriber('camera/rgb/image_color', Image, self.cameraCallback, queue_size=1)
self.image_pub = rospy.Publisher("turtle_env/camera_scaled", Image)
rospy.spin()
def cameraCallback(self, data):
if self.nPrints > 0:
print 'getting callback'
self.nPrints -= 1
try:
cv_image = self.bridge.imgmsg_to_cv2(data, 'rgb8')
except CvBridge, e:
print e
cv_image = cv2.resize(cv_image, (self.width, self.height))
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "rgb8"))
except CvBridgeError, e:
print e
class image_converter:
def __init__(self):
self.image_pub = rospy.Publisher("image_topic_2",Image)
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("ardrone/image_raw",Image,self.callback)
def callback(self,data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError, e:
print e
#(rows,cols,channels) = cv_image.shape
#if cols > 60 and rows > 60 :
# cv2.circle(cv_image, (50,50), 10, 255)
#cv2.imshow("Image window", cv_image)
#cv2.waitKey(3)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError, e:
print e
class motion_images :
history = None
nframes = 100
frame = 1
refPt=[]
windowhist=[]
comparehist=[]
dist=[]
path=[]
firstframe= None
i=0
index=0
def distance(self, p0, p1):
return math.sqrt((p0[0] - p1[0])**2 + (p0[1] - p1[1])**2)
def __init__(self, source1,source2,sink) :
self.image_pub = rospy.Publisher(sink, Image, queue_size=25)
self.bridge = CvBridge()
self.image_sub1 = rospy.Subscriber(source1, Image, self.callback1)
self.image_sub2 = rospy.Subscriber(source2, Image, self.callback2)
def callback1(self, data1) :
global cv_image1, cv_image2
try :
cv_image1 = self.bridge.imgmsg_to_cv2(data1, "bgr8")
except CvBridgeError, e:
print e
(rows,cols,channels) = cv_im
class face_position_publisher:
def __init__(self):
self.image_pub = rospy.Publisher("face_position", Point, queue_size=10)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("camera_node/image_raw",Image, self.callback, queue_size=10)
self.shutter_sub = rospy.Subscriber("/close_eye", Empty,self.save_callback, queue_size=10)
self.pub = rospy.Publisher('face/position', Point, queue_size=10)
self.image_publisher = rospy.Publisher('face/recognized', Image, queue_size=10)
self.cv_image = None
self.cnt = 0
def callback(self, data):
try:
self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
image_gray = cv2.cvtColor(self.cv_image, cv2.cv.CV_BGR2GRAY)
cascade = cv2.CascadeClassifier(cascade_path)
facerect = cascade.detectMultiScale(image_gray, scaleFactor=1.1, minNeighbors=10, minSize=(100, 100))
if len(facerect) > 0:
for rect in facerect:
cv2.rectangle(self.cv_image, tuple(rect[0:2]),tuple(rect[0:2]+rect[2:4]), color, thickness=2)
rospy.loginfo(rect)
self.pub.publish(Point(x=rect[0:1]+rect[1:2]/2,
y=rect[2:3]+rect[3:]/2,
z=0))
# self.image_publisher.publish(self.bridge.cv2_to_imgmsg(cv_image, 'bgr8'))
except CvBridgeError, e:
print(e)
def processFrame(image):
global pub1, pub2
conVerter = CvBridge()
#Convert from a ROS image to a CV image
try:
image = conVerter.imgmsg_to_cv2(image, "bgr8")
except CvBridgeError as e:
print (e)
result = bT.computeSpheres(image)
msgBallCoords = ballcoords()
msgModImage = Image()
if result == None:
return
msgBallCoords.color= "RED"
msgBallCoords.x = result[0]
msgBallCoords.y = result[1]
msgBallCoords.radius = result[2]
# convert from a CV image to a ROS image
resImage = result[3]
#resImage = conVerter.cv2_to_imgmsg(resImage , "bgr8")
msgModImage = conVerter.cv2_to_imgmsg(resImage , "bgr8")
rospy.loginfo(msgBallCoords)
pub1.publish(msgBallCoords)
pub2.publish(msgModImage)
class image_converter:
def __init__(self):
#self.image_pub = rospy.Publisher("image_topic_2",Image)
cv2.namedWindow("Image window", 1)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/camera/rgb/image_raw",Image,self.callback)
def callback(self,data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError, e:
print e
(rows,cols,channels) = cv_image.shape
#print rows, cols
rec_points=[(213,0), (213,480),(0,160),(640,160),(0,320),(640,320),(426,0),(426,480)]
i=0
while(i<len(rec_points)):
p1=rec_points[i]
p2=rec_points[i+1]
cv2.line(cv_image, p1, p2, (0,255,0), 1, 8, 0)
i=i+2
cv2.imshow("Image window", cv_image)
cv2.waitKey(3)
class smileDetect:
def __init__(self):
#pdb.set_trace()
self.image = None
self.image_pub = rospy.Publisher("image_topic_2",Image)
self.bridge = CvBridge()
self.camera_listener = rospy.Subscriber("camera/image_raw",Image, self.convertingNumpy)
print "initialized"
def convertingNumpy(self, msg):
print "converting image"
#reads in the images from the camera and converts it from ros image to openCV image
try:
cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError, e:
print e
self.image = numpy.asanyarray(cv_image)
print "self.image has length %s" %len(self.image)
#run facial detection algorithm
self.detectFaces()
cv2.imshow ("cv_image", self.image)
#need this to be in the same method and in the same indent as the cv2.imshow line
if cv2.waitKey(1) & 0xFF == ord('q'):
pass
class ROSInput(object):
"""
Capture video via ROS topic
"""
def __init__(self, input):
self._image = None
self._bridge = CvBridge()
rospy.Subscriber(input, Image, self.imageCallback)
def imageCallback(self, data):
try:
image_raw = self._bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
self._image = image_raw
def isActive(self):
return True
@property
def image(self):
return self._image
def cleanup(self):
pass
def isEnabled(self):
return self._enabled
def get_img(self, bag):
bridge = CvBridge()
for topic, msg, t in bag.read_messages(topics=['/cv_camera/image_raw/']):
img = bridge.imgmsg_to_cv2(msg, "bgr8")
rects = self.getCircles(img, 35)
for rect in rects:
yield rect
def on_enter(self,userdata):
bridge = CvBridge()
cv_image = bridge.imgmsg_to_cv2(userdata.Image, desired_encoding="passthrough")
self._filename = os.path.expanduser('~/picture_'+str(rospy.Time.now())+'.jpg')
print 'Saving file to ' , self._filename
cv2.imwrite(self._filename,cv_image)
print 'Picture has been saved to the home folder'
class RosTensorFlow():
def __init__(self):
classify_image.maybe_download_and_extract()
self._session = tf.Session()
classify_image.create_graph()
self._cv_bridge = CvBridge()
self._sub = rospy.Subscriber('image', Image, self.callback, queue_size=1)
self._pub = rospy.Publisher('result', String, queue_size=1)
self.score_threshold = rospy.get_param('~score_threshold', 0.1)
self.use_top_k = rospy.get_param('~use_top_k', 5)
def callback(self, image_msg):
cv_image = self._cv_bridge.imgmsg_to_cv2(image_msg, "bgr8")
# copy from
# classify_image.py
image_data = cv2.imencode('.jpg', cv_image)[1].tostring()
# Creates graph from saved GraphDef.
softmax_tensor = self._session.graph.get_tensor_by_name('softmax:0')
predictions = self._session.run(
softmax_tensor, {'DecodeJpeg/contents:0': image_data})
predictions = np.squeeze(predictions)
# Creates node ID --> English string lookup.
node_lookup = classify_image.NodeLookup()
top_k = predictions.argsort()[-self.use_top_k:][::-1]
for node_id in top_k:
human_string = node_lookup.id_to_string(node_id)
score = predictions[node_id]
if score > self.score_threshold:
rospy.loginfo('%s (score = %.5f)' % (human_string, score))
self._pub.publish(human_string)
def main(self):
rospy.spin()
class image_converter:
def __init__(self):
rospy.init_node('image_converter', anonymous=True)
self.image_pub = rospy.Publisher("analyzed_image", Image, queue_size=10)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("image_raw", Image, self.callback)
self.stabilizer = VideoStabilizer([[409, 250], [300, 762], [1123, 848], [1600, 238]])
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
#cv_image = self.analyze_image(cv_image)
#self.showImage(cv_image)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
def analyze_image(self, image):
image = self.stabilizer.stabilize_frame(image)
return image
def showImage(self, image):
cv2.imshow("Image window", image)
cv2.waitKey(3)
class BallTracker(object):
""" The BallTracker is a Python object that encompasses a ROS node
that can process images from the camera and search for a ball within.
The node will issue motor commands to move forward while keeping
the ball in the center of the camera's field of view. """
def __init__(self, image_topic):
""" Initialize the ball tracker """
rospy.init_node('ball_tracker')
self.cv_image = None # the latest image from the camera
self.bridge = CvBridge() # used to convert ROS messages to OpenCV
rospy.Subscriber(image_topic, Image, self.process_image)
self.pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
cv2.namedWindow('video_window')
def process_image(self, msg):
""" Process image messages from ROS and stash them in an attribute
called cv_image for subsequent processing """
self.cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
print self.cv_image.shape
cv2.imshow('video_window', self.cv_image)
cv2.waitKey(5)
def run(self):
""" The main run loop, in this node it doesn't do anything """
r = rospy.Rate(5)
while not rospy.is_shutdown():
# start out not issuing any motor commands
r.sleep()
class LineDetector:
def __init__(self, topic):
self.bridge = CvBridge()
self.cv_image = np.empty(shape=[0])
self.value_threshold = 180
self.image_width = 640
self.image_middle = 320
self.scan_height = 40
self.area_width, area_height = 20, 10
self.roi_vertical_pos = 300
self.row_begin = (self.scan_height - area_height) // 2
self.row_end = self.row_begin + area_height
self.pixel_cnt_threshold = 0.3 * self.area_width * area_height
self.detect_node = rospy.Subscriber(topic, Image, self.conv_image)
self.last_l, self.last_r = 0, 0
def conv_image(self, data):
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
frame = cv_image[self.roi_vertical_pos:self.roi_vertical_pos +
self.scan_height, :]
dst = cv2.Canny(frame, 50, 100)
cdst = cv2.cvtColor(dst, cv2.COLOR_GRAY2BGR)
lines = cv2.HoughLinesP(dst, 1, np.pi / 180, 50, None, 50, 5)
if lines is not None:
for i in range(0, len(lines)):
l = lines[i][0]
cv2.line(cdst, (l[0], l[1]), (l[2], l[3]), (0, 0, 255), 5,
cv2.LINE_AA)
lbound = np.array([0, 0, self.value_threshold], dtype=np.uint8)
ubound = np.array([131, 255, 255], dtype=np.uint8)
hsv = cv2.cvtColor(cdst, cv2.COLOR_BGR2HSV)
self.bin = cv2.inRange(hsv, lbound, ubound)
self.view = cv2.cvtColor(self.bin, cv2.COLOR_GRAY2BGR)
def detect_lines(self):
left, right = -1, -1
for l in range(self.image_middle, self.area_width, -1):
area = self.bin[self.row_begin:self.row_end, l - self.area_width:l]
if cv2.countNonZero(area) > self.pixel_cnt_threshold:
left = l
break
for r in range(self.image_middle, self.image_width - self.area_width):
area = self.bin[self.row_begin:self.row_end, r:r + self.area_width]
if cv2.countNonZero(area) > self.pixel_cnt_threshold:
right = r
break
if right > 0 and left > 0 and abs(right - left) < 450:
if self.last_r == -1:
right = -1
elif self.last_l == -1:
left = -1
self.last_l = left
self.last_r = right
return left, right
def show_images(self, left, right):
if left != -1:
self.lsquare = cv2.rectangle(
self.view, (left, self.row_begin),
(left - self.area_width, self.row_end), (255, 255, 0), 3)
# else:
# print("")#Lost left line
if right != -1:
self.rsquare = cv2.rectangle(
self.view, (right, self.row_begin),
(right + self.area_width, self.row_end), (0, 255, 255), 3)
# else:
# print("")#Lost right line
cv2.imshow('view', self.view)
# print(right, left)
cv2.waitKey(1)
class DemoNode(object):
def __init__(self):
self._cv_br = CvBridge()
self._ros_init()
self._roi_lock = Lock()
self._latest_rois = []
self._depth_lock = Lock()
self._latest_depth = None
def _ros_init(self):
rospy.init_node('debug')
rospy.Subscriber('/detector/detections', ClassifiedRegionsOfInterest, self._detect_callback, queue_size=1)
rospy.Subscriber(rospy.get_param('image_topic', '/camera/color/image_raw'), Image, self._camera_callback, queue_size=2)
rospy.Subscriber(rospy.get_param('depth_topic', '/camera/depth/image_rect_raw'), Image, self._depth_callback, queue_size=2)
self._publisher = rospy.Publisher('demo', Image, queue_size=2)
def _detect_callback(self, regions):
with self._roi_lock:
self._latest_rois = regions.regions
def _depth_callback(self, depth):
with self._depth_lock:
self._latest_depth = self._cv_br.imgmsg_to_cv2(depth)
def _camera_callback(self, image):
frame = self._cv_br.imgmsg_to_cv2(image)
rois = []
with self._roi_lock:
rois = self._latest_rois
depth = None
with self._depth_lock:
depth = self._latest_depth
for roi in rois:
cv2.rectangle(frame, (roi.x, roi.y),
(roi.x + roi.w, roi.y + roi.h), (0, 255, 0), 10)
if depth is not None:
y_scale = depth.shape[0] / frame.shape[0]
x_scale = depth.shape[1] / frame.shape[1]
d_x = int(roi.x * x_scale)
d_y = int(roi.y * y_scale)
d_w = int(roi.w * x_scale)
d_h = int(roi.h * y_scale)
sub_depth = depth[d_y:d_y+d_h,d_x:d_x+d_w]
median_distance = np.median(sub_depth) / 1000.0
cv2.putText(frame, "%.1f m" % median_distance, (max(0, roi.x), max(0, roi.y - 20)), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2, cv2.LINE_AA)
self._publisher.publish(self._cv_br.cv2_to_imgmsg(cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)))
class ObjectTracking(object):
def __init__(self, SPR_opt, init_estimate):
self.SPR_opt = SPR_opt
self.init_estimate = init_estimate
self.estimate = init_estimate
self.target = init_estimate
self.pub = rospy.Publisher('/spr/dlo_estimation', PointCloud, queue_size=3)
# object that listen to transformation tree.
self.tf_listener = tf.TransformListener()
self.target_num_points = 300
self.min_num_points = self.target_num_points/3
self.voxel_size = 15
self.offset_xyz = np.array([-0.045,0,0]) # offset in x,y,z in meters
# camera properites
self.camera_properties = {
'res_color_height': 720,
'res_color_width': 1280,
'res_depth_height': 480,
'res_depth_width': 640,
'color_K': np.array([[919.5044555664062, 0.0, 645.03076171875,],\
[0.0, 919.5109252929688, 357.19921875],[0.0, 0.0, 1.0]]),
'depth_K': np.array([[385.37188720703125, 0.0, 320.0655517578125],\
[0.0, 385.37188720703125, 241.86325073242188],[0.0, 0.0, 1.0]]),
'depth_reshape': np.array([[1],[640]])
}
# from ros image to opencv image
self.bridge = CvBridge()
self.pcd = o3d.geometry.PointCloud() #creating point cloud
# set to something else if apriltags are not used and manual calibration is desierd.
self.tag_quaternion_xyzw = np.zeros(4)
self.tag_translation_xyz = np.zeros(3)
self.rotation_matrix = np.eye(3)
self.num_calibration_itter = 0
# filter table
self.height_off = -0.004 # m offest for which all points under get filtered away
def callback(self, depth_data, rgb_data):
time_start = time.time()
try:
color_img = self.bridge.imgmsg_to_cv2(rgb_data, rgb_data.encoding)
except CvBridgeError as e:
print(e)
self.stamp = rgb_data.header.stamp
self.frame_id = rgb_data.header.frame_id
# keep the 16 bit information
depth_vec = np.frombuffer(depth_data.data, dtype=np.uint16)
depth_img = depth_vec.reshape((self.camera_properties['res_depth_height'], \
self.camera_properties['res_depth_width']))
depth_img = depth_img/1000 # from milimeter int to float meter.
# mask cable
black_lower = (0, 0, 0)
black_upper = (180, 255, 25)
blue_lower = (100, 235, 50)
blue_upper = (120, 255, 255)
hsv_image = cv2.cvtColor(color_img, cv2.COLOR_RGB2HSV)
mask = cv2.inRange(hsv_image, blue_lower, blue_upper)
# get coord
[row,col]=np.nonzero(mask)
p_color = np.transpose(np.array([col, row, np.ones(np.size(row))]))
# down sample
self.pcd.points = o3d.utility.Vector3dVector(p_color)
self.pcd = self.pcd.voxel_down_sample(voxel_size=self.voxel_size)
#homogeneous coordinates
K_inv = np.linalg.pinv(self.camera_properties['color_K'])
p_homogeneous = np.transpose(K_inv.dot(np.asarray(self.pcd.points).T))
p_homogeneous_offset = p_homogeneous# + self.offset_xyz
#cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
#cv2.imshow('RealSense', depth_img)
#cv2.imshow('RealSense', mask)
#cv2.waitKey(1)
if np.shape(p_homogeneous)[0] > self.min_num_points:
# dynamic downsample
down_samlpe = np.shape(p_homogeneous)[0] / self.target_num_points
if down_samlpe < 0.95:
self.voxel_size -= 0.2
elif down_samlpe > 1.05:
self.voxel_size += 0.2
# to depth camera
p_depth = np.transpose(self.camera_properties['depth_K'].dot(p_homogeneous_offset.T))
# round to nearest int
p_depth_idx = np.rint(p_depth)
indices = p_depth_idx[:,0:2].astype(int).dot(self.camera_properties['depth_reshape'])
# clip indecies within range, maybe implement a better version in future
indices = indices.clip(0, \
(self.camera_properties['res_depth_height']*self.camera_properties['res_depth_width']-1))
depth_z = depth_img.reshape(-1)[indices]
# calculate point cloud
point_cloud = p_homogeneous*depth_z
# remove non vaild points
point_cloud = point_cloud[~np.all(point_cloud == 0, axis=1)]
# get transformation matrix
(trans, rot) = self.tf_listener.lookupTransform('/yumi_base_link', '/camera_color_optical_frame', rospy.Time(0))
transformer_ = tf.TransformerROS(True, rospy.Duration(1.0))
tf_matrix = transformer_.fromTranslationRotation(translation=trans, rotation=rot)
# homogenious coordinates
col_ones = np.ones((np.shape(point_cloud)[0],1))
point_cloud = np.hstack((point_cloud,col_ones))
# Transform
point_cloud = np.transpose(tf_matrix.dot(point_cloud.T))
# flattend
point_cloud = point_cloud[:,0:3]/point_cloud[:,3:4]
# remove point that are under certatin hight from the worktable
index_ = point_cloud[:,2] >= self.height_off
point_cloud = point_cloud[index_,:]
self.target = point_cloud
# plot depthimage and slected points for calibration
#depth_img.reshape(-1)[indices] = 255
#cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
#cv2.imshow('RealSense', depth_img)
#cv2.imshow('RealSense', mask)
#cv2.waitKey(1)
# call SPR
SPR_Transform = spr_local.SPR_register(self.target, self.estimate, self.SPR_opt) # CPD warp Y to X, fliped! X_warp = SPR_Transform.Y;
self.estimate = SPR_Transform.get('Y')
time_update = time.time() - time_start
print('time_update ', time_update)
self.publish_msg()
def publish_msg(self):
cloudpoints = self.estimate
cloud_msg = PointCloud()
cloud_msg.header.stamp = self.stamp
cloud_msg.header.frame_id = "yumi_base_link"
for i in range(cloudpoints.shape[0]):
cloud_msg.points.append(Point32(cloudpoints[i, 0], cloudpoints[i, 1], cloudpoints[i, 2]))
# Change to camera frame
self.pub.publish(cloud_msg)
def update_transformation(self):
pass
def update_calibration_color(self, camera_info):
K = np.array(camera_info.K)
self.camera_properties['color_K'] = K.reshape((3, 3))
self.camera_properties['res_color_width'] = camera_info.width
self.camera_properties['res_color_height'] = camera_info.height
def update_calibration_depth(self, camera_info):
K = np.array(camera_info.K)
self.camera_properties['depth_K'] = K.reshape((3, 3))
self.camera_properties['res_depth_width'] = camera_info.width
self.camera_properties['res_depth_height'] = camera_info.height
self.camera_properties['depth_reshape'] = np.array([[1],[camera_info.width]])
class image_handler:
# Constructor
def __init__(self):
self.pub_det = rospy.Publisher("image/detection", Image)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/camera/image_raw", Image,
self.callback)
self.curr_kp = None
self.curr_desc = None
self.train_kp = None
self.train_desc = None
self.detect = False
self.img_train = None
self.referencePoints = []
self.cropping = False
# Callback
def callback(self, data):
# recibimos una nueva imagen como 'data' y la convertimos en imagen de opencv
try:
self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# obtener dimensiones de la imagen
(rows, cols, channels) = self.cv_image.shape
# pasar a escala de grises
gray = cv2.cvtColor(self.cv_image, cv2.COLOR_BGR2GRAY)
#-------------------------------------------------------
# Feature Detector and Description
self.detector = cv2.xfeatures2d.SIFT_create()
#self.detector = cv2.xfeatures2d.SURF_create()
#self.detector = cv2.KAZE_create()
#self.detector = cv2.AKAZE_create()
#self.detector = cv2.BRISK_create()
#self.detector = cv2.ORB_create()
(self.curr_kp,
self.curr_desc) = self.detector.detectAndCompute(gray, None)
img_kp = cv2.drawKeypoints(self.cv_image,
self.curr_kp,
None,
color=(0, 0, 255))
cv2.imshow('image', img_kp)
cv2.setMouseCallback('image', self.clipAndCrop)
# eventos de teclado
k = cv2.waitKey(30)
#-------------------------------------------------------
# Image matching
if self.detect:
# creamos objeto que realiza matching de fuerza bruta (Brute Force Matching, BFMatcher)
# OJO: cuidado con el tipo de descriptor (binario o punto flotante)
#bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
bf = cv2.BFMatcher(cv2.NORM_L2, crossCheck=True)
# Matching de descriptores
matches = bf.match(self.train_desc, self.curr_desc)
# Los ordenamos en funcion de su distancia
matches = sorted(matches, key=lambda x: x.distance)
N = len(matches)
M = int(math.floor(
0.6 *
N)) # seleccionaremos el 60% de los puntos con menor distancia
if M > MIN_MATCH_COUNT:
train_pts = np.float32([
self.train_kp[m.queryIdx].pt for m in matches[:M]
]).reshape(-1, 1, 2)
curr_pts = np.float32([
self.curr_kp[m.trainIdx].pt for m in matches[:M]
]).reshape(-1, 1, 2)
H, mask = cv2.findHomography(train_pts, curr_pts, cv2.RANSAC,
5.0)
matchesMask = mask.ravel().tolist()
h, w, d = self.train_img.shape
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
[w - 1, 0]]).reshape(-1, 1, 2)
dst = cv2.perspectiveTransform(pts, H)
img_det = cv2.polylines(self.cv_image, [np.int32(dst)], True,
(255, 255, 255), 3, cv2.LINE_AA)
else:
print "Not enough matches are found - %d/%d" % (
M, MIN_MATCH_COUNT)
matchesMask = None
# parametros de dibujo
draw_params = dict(
matchColor=(0, 255, 0), # dibujar matches en verde
singlePointColor=None,
matchesMask=matchesMask, # dibujar solo inliers
flags=2)
img_det2 = cv2.drawMatches(self.train_img, self.train_kp, img_det,
self.curr_kp, matches[:M], None,
**draw_params)
# publicamos la imagen al topico /image/detection
try:
self.pub_det.publish(
self.bridge.cv2_to_imgmsg(img_det2, "bgr8"))
except CvBridgeError as e:
print('Homography')
print(e)
def clipAndCrop(self, event, x, y, flags, param):
#referencia: http://www.pyimagesearch.com/2015/03/09/capturing-mouse-click-events-with-python-and-opencv/
# if the left mouse button was clicked, record the starting
# (x, y) coordinates and indicate that cropping is being
# performed
if event == cv2.EVENT_LBUTTONDOWN:
self.referencePoints = [(x, y)]
self.cropping = True
# check to see if the left mouse button was released
elif (event == cv2.EVENT_LBUTTONUP and self.cropping):
# record the ending (x, y) coordinates and indicate that
# the cropping operation is finished
self.referencePoints.append((x, y))
cropping = False
img_clone = self.cv_image.copy()
# draw a rectangle around the region of interest
cv2.rectangle(img_clone, self.referencePoints[0],
self.referencePoints[1], (0, 255, 0), 2)
# calcular descriptores en imagen de entrenamiento
self.train_img = img_clone[
self.referencePoints[0][1]:self.referencePoints[1][1],
self.referencePoints[0][0]:self.referencePoints[1][0]]
train_gray = cv2.cvtColor(self.train_img, cv2.COLOR_BGR2GRAY)
(self.train_kp, self.train_desc) = self.detector.detectAndCompute(
train_gray, None)
self.detect = True
print('Imagen de entrenamiento capturada!')
cv2.imshow('image', img_clone)
class DepthRegisterNode(object):
'''class holding and performing ROS-related stuff like subscriptions, publishing, parameters and callbacks'''
def __init__(self):
rospy.init_node('simple_depth_register_node')
self.cv_bridge = CvBridge()
# extrinsics parameters (offset rgb cam -> depth cam), defaulting to realsense r200 values
dx = rospy.get_param('~x_offset', -0.0589333333)
dy = rospy.get_param('~y_offset', 0)
dz = rospy.get_param('~z_offset', 0)
# scale in which depth values are presented relative to meters
depth_scale = rospy.get_param('~depth_scale', 1000.0) # 1000 is millimeters
self.dr = DepthRegisterer(dx, dy, dz, depth_scale)
# input topics
rgb_topic = rospy.get_param('~rgb_topic', '/camera/rgb/image_raw')
depth_topic = rospy.get_param('~depth_topic', '/camera/depth/image_raw')
# read cameras' intrinsics
rgb_info_topic, depth_info_topic = [t[:t.rfind('/')] + '/camera_info' for t in (rgb_topic, depth_topic)]
self.camera_info_callback(rospy.wait_for_message(rgb_info_topic, CameraInfo), 'rgb')
self.camera_info_callback(rospy.wait_for_message(depth_info_topic, CameraInfo), 'depth')
rospy.loginfo('camera calibration data OK')
# subscribe to input topics
rgb_sub = message_filters.Subscriber(rgb_topic, Image)
depth_sub = message_filters.Subscriber(depth_topic, Image)
self.sub = message_filters.ApproximateTimeSynchronizer([rgb_sub, depth_sub], 3, 0.1)
self.sub.registerCallback(self.image_pair_callback)
rospy.loginfo('synchronized subscriber OK')
# registered depth image output topic name
registered_topic = rospy.get_param('~registered_topic', '~depth_registered')
# announce output topics
self.pub = rospy.Publisher(registered_topic, Image, queue_size=5)
self.pub_info_unregistered = rospy.Publisher('~info_image_unregistered', Image, queue_size=5)
self.pub_info_registered = rospy.Publisher('~info_image_registered', Image, queue_size=5)
self.rgb_image = None
self.depth_image = None
self.registered_depth_image = None
def camera_info_callback(self, msg_camera_info, cam_id):
'''passes the intrinsics of a camera_info message to the depth registerer'''
fx, _, cx, _, fy, cy, _, _, _ = msg_camera_info.K
self.dr.set_intrinsics(cam_id, fx, fy, cx, cy)
def image_pair_callback(self, msg_rgb_image, msg_depth_image):
'''makes the depth registerer process the image pair and produces output images'''
# convert images ROS -> OpenCV
try:
self.rgb_image = self.cv_bridge.imgmsg_to_cv2(msg_rgb_image, "rgb8")
except CvBridgeError as e:
rospy.logwarn('error converting rgb image: %s' % e)
return
try:
self.depth_image = self.cv_bridge.imgmsg_to_cv2(msg_depth_image)
except CvBridgeError as e:
rospy.logwarn('error converting depth image: %s' % e)
return
# be lazy, check subscriber count first
has_depth_subs = self.pub.get_num_connections() > 0
has_info_subs = (self.pub_info_unregistered.get_num_connections() + self.pub_info_registered.get_num_connections()) > 0
has_subs = has_depth_subs or has_info_subs
if has_subs:
# processing
self.registered_depth_image = self.dr.register(self.rgb_image, self.depth_image)
# convert image OpenCV -> ROS and send out
msg = self.cv_bridge.cv2_to_imgmsg(self.registered_depth_image)
msg.header.stamp = msg_depth_image.header.stamp
self.pub.publish(msg)
if has_info_subs:
# send out info images
img_unregistered_img, img_registered_img = self.get_info_images()
self.pub_info_unregistered.publish(self.cv_bridge.cv2_to_imgmsg(img_unregistered_img, "rgb8"))
self.pub_info_registered.publish(self.cv_bridge.cv2_to_imgmsg(img_registered_img, "rgb8"))
def get_info_images(self):
'''generates one registered and one unregistered blended image of rgb and depth image'''
# clip depth values to a certain maximum and scale constantly wrt. this value to avoid jitter
clipping_distance = 5. # meters
# clip
depth_image = self.depth_image / (clipping_distance * self.dr.depth_scale) * 255.
depth_image[depth_image > 255] = 255
# resize to fit rgb image and convert to 24-bit bgr
depth_image = cv2.cvtColor(cv2.resize(depth_image.astype('uint8'), (self.rgb_image.shape[1], self.rgb_image.shape[0]), interpolation=cv2.INTER_NEAREST), cv2.COLOR_GRAY2BGR)
# clip
registered_depth_image = self.registered_depth_image.astype(float) * 255. / clipping_distance
registered_depth_image[registered_depth_image > 255] = 255
# convert to 24-bit bgr
registered_depth_image = cv2.cvtColor(registered_depth_image.astype('uint8'), cv2.COLOR_GRAY2BGR)
# blend
return cv2.addWeighted(self.rgb_image, 0.5, depth_image, 0.5, 0), cv2.addWeighted(self.rgb_image, 0.5, registered_depth_image, 0.5, 0)
def spin(self):
'''stayin' alive'''
rospy.spin()
class PersonDetector():
def __init__(self):
# cv_bridge handles
self.cv_bridge = CvBridge()
self.person_bbox = BoundingBox()
# ROS PARAM
self.m_pub_threshold = rospy.get_param('~pub_threshold', 0.70)
# detect width height
self.WIDTH = 50
self.HEIGHT = 50
# Publish
self.pub_cmd = rospy.Publisher('cmd_vel', Twist, queue_size=10)
# Twist 型のデータ
self.t = Twist()
# Subscribe
sub_camera_rgb = rospy.Subscriber('/camera/color/image_raw', Image, self.CamRgbImageCallback)
sub_camera_depth = rospy.Subscriber('/camera/aligned_depth_to_color/image_raw', Image, self.CamDepthImageCallback)
sub_darknet_bbox = rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, self.DarknetBboxCallback)
return
def CamRgbImageCallback(self, rgb_image_data):
try:
rgb_image = self.cv_bridge.imgmsg_to_cv2(rgb_image_data, 'passthrough')
except CvBridgeError, e:
rospy.logerr(e)
rgb_image.flags.writeable = True
rgb_image = cv2.cvtColor(rgb_image, cv2.COLOR_BGR2RGB)
h, w, c = rgb_image.shape
# 人がいる場合
if self.person_bbox.probability > 0.0:
x_person = (int)(self.person_bbox.xmax+self.person_bbox.xmin)/2
y_person = (int)(self.person_bbox.ymax+self.person_bbox.ymin)/2
x1 = (w / 2) - self.WIDTH
x2 = (w / 2) + self.WIDTH
y1 = (h / 2) - self.HEIGHT
y2 = (h / 2) + self.HEIGHT
sum = 0.0
for i in range(y1, y2):
for j in range(x1, x2):
rgb_image.itemset((i, j, 0), 0)
rgb_image.itemset((i, j, 1), 0)
if self.m_depth_image.item(i,j) == self.m_depth_image.item(i,j):
sum += self.m_depth_image.item(i,j)
ave = sum / ((self.WIDTH * 2) * (self.HEIGHT * 2))
#rospy.loginfo('Class : person, Score: %.2f, Dist: %dmm ' %(self.person_bbox.probability, ave))
print("%f [m]" % ave)
velocity = 0
rotation = 0
error_angle = x_person - 320
error_distance = ave - 150
controll_angle_gain = 0.006
controll_distance_gain = 0.0004
if error_angle > 0 and ave > 300:
velocity = controll_distance_gain * error_distance
if velocity > 0.2:
velocity = 0.2
rotation = -controll_angle_gain * error_angle
elif error_angle < 0 and ave > 300:
velocity = controll_distance_gain * error_distance
if velocity > 0.2:
velocity = 0.2
rotation = -controll_angle_gain * error_angle
else:
velocity = 0.0
rotation = 0.0
self.t.linear.x = velocity
self.t.angular.z = rotation
self.pub_cmd.publish(self.t)
"""
cv2.normalize(self.m_depth_image, self.m_depth_image, 0, 1, cv2.NORM_MINMAX)
cv2.namedWindow("color_image")
cv2.namedWindow("depth_image")
cv2.imshow("color_image", rgb_image)
cv2.imshow("depth_image", self.m_depth_image)
cv2.waitKey(10)
"""
"""
cv2.rectangle(rgb_image, (self.person_bbox.xmin, self.person_bbox.ymin), (self.person_bbox.xmax, self.person_bbox.ymax),(0,0,255), 2)
#rospy.loginfo('Class : person, Score: %.2f, Dist: %dmm ' %(self.person_bbox.probability, m_person_depth))
text = "person " +('%dmm' % ave)
text_top = (self.person_bbox.xmin, self.person_bbox.ymin - 10)
text_bot = (self.person_bbox.xmin + 80, self.person_bbox.ymin + 5)
text_pos = (self.person_bbox.xmin + 5, self.person_bbox.ymin)
cv2.rectangle(rgb_image, text_top, text_bot, (0,0,0),-1)
cv2.putText(rgb_image, text, text_pos, cv2.FONT_HERSHEY_SIMPLEX, 0.35, (255, 0, 255), 1)
"""
"""
cv2.namedWindow("rgb_image")
cv2.imshow("rgb_image", rgb_image)
cv2.waitKey(10)
cv2.normalize(self.m_depth_image, self.m_depth_image, 0, 32768, cv2.NORM_MINMAX)
cv2.namedWindow("depth_image")
cv2.imshow("depth_image", self.m_depth_image)
cv2.waitKey(10)
"""
return
def get_distance(img):
global depth_image
bridge = CvBridge()
depth_image = bridge.imgmsg_to_cv2(img, desired_encoding="32FC1")
class image_converter:
def __init__(self):
self.image_pub = rospy.Publisher("trackedBall/camera1",
Image,
queue_size=10)
self.location_pub = rospy.Publisher("locationBall/camera1",
String,
queue_size=10)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("camera1/camera", Image,
self.callback)
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
#define boundaries of color (in this case green, HSV)
greenLower = (29, 86, 6)
greenUpper = (64, 255, 255)
#resize for easier manipulation
cv_image = imutils.resize(cv_image, width=600)
#option to blur if needed
# blurred = cv2.GaussianBlur(cv_image, (11, 11), 0)
#recolor to HSV
hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
#construct masks for the green color, then remove blobs
mask = cv2.inRange(hsv, greenLower, greenUpper)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
#find contours in mask and initialize center of ball (x,y)
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
#proceed if at least one contour was found.
if len(cnts) > 0:
# find the largest contour in the mask, then use
# it to compute the minimum enclosing circle and
# centroid
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
# only proceed if radius meets a minimum size
if radius > 10:
# draw the circle and centroid on the frame,
# then update the list of tracked points
cv2.circle(cv_image, (int(x), int(y)), int(radius),
(0, 255, 255), 2)
cv2.circle(cv_image, center, 3, (0, 0, 255), -1)
cv2.imshow("Image window", cv_image)
cv2.waitKey(3)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
self.location_pub.publish(center)
except CvBridgeError as e:
print(e)
class TLDetector(object):
''' Contructor.
'''
def __init__(self):
rospy.init_node('tl_detector')
self.use_ground_truth = USE_GROUND_TRUTH
# Traffic light classifier's thread and its task queue.
self.image_processing = False
self.queue = Queue()
self.light_classifier = TLClassifier(self.queue, self.classified_cb)
self.light_classifier.daemon = True
self.light_classifier.start()
self.config = yaml.load(rospy.get_param("/traffic_light_config"))
self.cv_bridge = CvBridge()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.next_waypoint_ahead = None
self.waypoints = None
self.traffic_lights = RouteTrafficLights()
# Subscribers.
sub0 = rospy.Subscriber('/next_waypoint_ahead', Int32, self.wp_cb)
sub1 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
sub2 = rospy.Subscriber('/image_color', Image, self.image_cb)
'''
/vehicle/traffic_lights provides the location of the traffic light in 3D
map space and helps to acquire an accurate ground truth data source for
the traffic light classifier by sending the current color state of all
traffic lights in the simulator. When testing on the vehicle, the color
state will not be available.
'''
sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
# Publishers.
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
rospy.spin()
def wp_cb(self, msg):
''' Dispatches a message from topic /next_waypoint_ahead.
Args:
msg (Int32): Index of the next waypoint ahead.
'''
self.next_waypoint_ahead = msg.data
def waypoints_cb(self, waypoints):
''' Dispatches a message from topic /base_waypoints.
Args:
waypoints (Lane): Waypoints of the track.
'''
self.waypoints = list(waypoints.waypoints)
# Reset traffic light information on each route update.
self.traffic_lights.reset(self.waypoints,
self.config['stop_line_positions'])
def traffic_cb(self, msg):
''' Dispatches a message from topic /vehicle/traffic_lights.
Args:
msg (TrafficLightArray): Traffic light data.
'''
self.traffic_lights.update_states(msg.lights)
def image_cb(self, msg):
''' Dispatches a message from topic /image_color.
Args:
msg (Image): image from car-mounted camera.
'''
light_wp = -1
light_state = TrafficLight.UNKNOWN
light_key = None
if self.next_waypoint_ahead:
# Get next traffic light ahead.
light_wp, light_key = \
self.traffic_lights.get_next_en_route(self.next_waypoint_ahead)
if light_key == None:
# Next light key is unknown so as the light state.
self.publish_red_light(light_wp, light_state)
else:
# Distance in waypoints in a circular track.
d = lambda x, y, s: (x - y + s) % s
stop_point = self.traffic_lights[light_key].stop_point
if d(stop_point, self.next_waypoint_ahead, len(self.waypoints)) \
> LIGHT_DETECTION_DISTANCE:
# Next traffic light is too far, its state is unknown.
self.publish_red_light(light_wp, light_state)
else:
# Get state of next waypoint from either simulator or image.
if self.use_ground_truth:
self.detect_light_state_from_gt(light_wp, light_key)
else:
self.detect_light_state_from_img(light_wp, msg)
def classified_cb(self, light_wp, light_state):
''' Dispatches the traffic light classification result.
Args:
light_wp (Int32): Index of the traffic light's waypoint.
light_state (TrafficLight): State of the traffic light.
'''
self.image_processing = False
self.publish_red_light(light_wp, light_state)
def detect_light_state_from_img(self, light_wp, image_msg):
''' Detects and classifies traffic lights in the image message.
Args:
light_wp (Int32): Index of the traffic light's waypoint.
image_msg (Image): Image from car-mounted camera.
'''
if not self.image_processing:
image = self.cv_bridge.imgmsg_to_cv2(image_msg, "bgr8")
self.image_processing = True
self.queue.put((light_wp, image))
def detect_light_state_from_gt(self, light_wp, light_key):
''' Detects and classifies traffic lights from the ground truth.
Args:
light_wp (Int32): Index of the traffic light's waypoint.
light_key: Index of the traffic light.
'''
light_state = self.traffic_lights[light_key].state
self.publish_red_light(light_wp, light_state)
def publish_red_light(self, light_wp, light_state):
''' Publishes the index of the waypoint closest to the red light's stop
line to /traffic_waypoint. Each predicted state has to occur
STATE_COUNT_THRESHOLD number of times till we start using it.
Otherwise the previous stable state is used.
Args:
light_wp (Int32): Index of the traffic light's waypoint.
light_state (TrafficLight): State of the traffic light.
'''
rospy.logdebug("publish_red_light: light_wp %d, light_state %d",
light_wp, light_state)
if self.state != light_state:
self.state_count = 0
self.state = light_state
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
light_wp = light_wp \
if light_state in (TrafficLight.RED, TrafficLight.YELLOW) \
else -1
self.last_wp = light_wp
self.upcoming_red_light_pub.publish(Int32(light_wp))
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
self.state_count += 1
class DepthController:
MM_IN_M = 1000
def __init__(
self,
distance_threshold,
num_angles,
sphere_radius,
step_len,
debug,
steer_multiplier,
horizontal_stretch,
):
self.distance_threshold = distance_threshold
self.num_angles = num_angles
self.sphere_radius = sphere_radius
self.step_len = step_len
self.debug = debug
self.steer_multiplier = steer_multiplier
self.horizontal_stretch = horizontal_stretch
self.pub_servo = rospy.Publisher('/servo', Int16, queue_size=1)
self.pub_esc = rospy.Publisher('/esc', Int16, queue_size=1)
self.bridge = CvBridge()
rospy.Subscriber('/camera/aligned_depth_to_color/image_raw', Image,
self._depth_cb)
if self.debug:
self.pub_debug_image = rospy.Publisher('/debug_image',
Image,
queue_size=1)
self.servo_neutral = 1479
self.servo_range = 500
self.esc_neutral = 1500
self.angle_mult = 600 #80 # 600
self.throt_mult = 200 # 80 # 200
rospy.Subscriber('/joy', Joy, self._joy_cb, queue_size=1)
rospy.init_node('depth_controller')
def _depth_cb(self, msg):
self.depth_frame = self.bridge.imgmsg_to_cv2(
msg, desired_encoding='passthrough').astype(
np.float) / self.MM_IN_M
def spin(self):
rate = rospy.Rate(100)
while not rospy.is_shutdown():
self.choose_angle()
def choose_angle(self):
if self.depth_frame is None:
return
# Makes the code a bit less cluttered
depth_frame = self.depth_frame
depth_frame[depth_frame > self.distance_threshold] = np.nan
depth_frame[depth_frame == 0] = np.nan
start_time = time.time()
waypoints, frame = find_waypoints(
depth_frame,
horizontal_stretch=self.horizontal_stretch,
statistic=np.nanmean,
num_angles=self.num_angles,
step_len=self.step_len,
sphere_radius=self.sphere_radius,
draw_waypoints=self.debug,
)
if self.debug:
delta = time.time() - start_time
rospy.loginfo('Finding waypoints took: {:.1f}ms'.format(1000 *
delta))
# Also, publish an image for debugging
depth_frame[np.isnan(depth_frame)] = 0
depth_frame -= depth_frame.min()
depth_frame = 127 * (depth_frame / depth_frame.max())
debug_image = image.numpy_to_image(depth_frame.astype('uint8'),
encoding='mono8')
self.pub_debug_image.publish(debug_image)
start_time = time.time()
angle = find_angle(waypoints)
if self.debug:
delta = time.time() - start_time
rospy.loginfo('Finding angle took: {:.1f}ms'.format(1000 * delta))
angle = self.servo_neutral + int(
self.steer_multiplier * angle * self.servo_range)
self.pub_servo.publish(angle)
def _joy_cb(self, msg):
throt = msg.axes[3]
throt = self.esc_neutral + int(self.throt_mult * throt)
self.pub_esc.publish(throt)
def edge_callback(self, data):
bridge = CvBridge()
global edge_image
edge_image = bridge.imgmsg_to_cv2(data, "mono8")
class AllSensorBot(object):
def __init__(self,
use_lidar=False, use_camera=False, use_imu=False,
use_odom=False, use_joint_states=False):
# velocity publisher
self.vel_pub = rospy.Publisher('cmd_vel', Twist,queue_size=1)
# lidar scan subscriber
if use_lidar:
self.scan = LaserScan()
self.lidar_sub = rospy.Subscriber('scan', LaserScan, self.lidarCallback)
# camera subscribver
# please uncoment out if you use camera
if use_camera:
# for convert image topic to opencv obj
self.img = None
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber('image_raw', Image, self.imageCallback)
# imu subscriber
if use_imu:
self.imu_sub = rospy.Subscriber('imu', Imu, self.imuCallback)
# odom subscriber
if use_odom:
self.odom_sub = rospy.Subscriber('odom', Odometry, self.odomCallback)
# joint_states subscriber
if use_joint_states:
self.odom_sub = rospy.Subscriber('joint_states', JointState, self.jointstateCallback)
def strategy(self):
'''
calc Twist and publish cmd_vel topic
'''
r = rospy.Rate(1)
while not rospy.is_shutdown():
# update twist
twist = Twist()
//twist.linear.x = 0.1; twist.linear.y = 0; twist.linear.z = 0
//twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
# publish twist topic
self.vel_pub.publish(twist)
r.sleep()
# lidar scan topic call back sample
# update lidar scan state
def lidarCallback(self, data):
self.scan = data
rospy.loginfo(self.scan)
# camera image call back sample
# comvert image topic to opencv object and show
def imageCallback(self, data):
try:
self.img = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
rospy.logerr(e)
cv2.imshow("Image window", self.img)
cv2.waitKey(1)
# imu call back sample
# update imu state
def imuCallback(self, data):
self.imu = data
rospy.loginfo(self.imu)
# odom call back sample
# update odometry state
def odomCallback(self, data):
self.pose_x = data.pose.pose.position.x
self.pose_y = data.pose.pose.position.y
rospy.loginfo("odom pose_x: {}".format(self.pose_x))
rospy.loginfo("odom pose_y: {}".format(self.pose_y))
# jointstate call back sample
# update joint state
def jointstateCallback(self, data):
self.wheel_rot_r = data.position[0]
self.wheel_rot_l = data.position[1]
rospy.loginfo("joint_state R: {}".format(self.wheel_rot_r))
rospy.loginfo("joint_state L: {}".format(self.wheel_rot_l))
class Detector:
def __init__(self):
self.image_pub = rospy.Publisher("debug_image",Image, queue_size=1)
self.object_pub = rospy.Publisher("~objects", Detection2DArray, queue_size=1)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("image", Image, self.image_cb, queue_size=1, buff_size=2**24)
self.sess = tf.Session(graph=detection_graph,config=config)
# load the label map into the ROS parameter server as a list of strings of labels
rospy.set_param( "~labels", {str(c['id']): c['name'] for c in categories} )
# advertise that the detector node is ready
rospy.Service( 'check_is_ready', CheckIsReady, self.check_is_ready )
def check_is_ready(self, req):
return True
def image_cb(self, data):
objArray = Detection2DArray()
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
image=cv2.cvtColor(cv_image,cv2.COLOR_BGR2RGB)
# the array based representation of the image will be used later in order to prepare the
# result image with boxes and labels on it.
image_np = np.asarray(image)
# Expand dimensions since the model expects images to have shape: [1, None, None, 3]
image_np_expanded = np.expand_dims(image_np, axis=0)
image_tensor = detection_graph.get_tensor_by_name('image_tensor:0')
# Each box represents a part of the image where a particular object was detected.
boxes = detection_graph.get_tensor_by_name('detection_boxes:0')
# Each score represent how level of confidence for each of the objects.
# Score is shown on the result image, together with the class label.
scores = detection_graph.get_tensor_by_name('detection_scores:0')
classes = detection_graph.get_tensor_by_name('detection_classes:0')
num_detections = detection_graph.get_tensor_by_name('num_detections:0')
(boxes, scores, classes, num_detections) = self.sess.run([boxes, scores, classes, num_detections],
feed_dict={image_tensor: image_np_expanded})
objects=vis_util.visualize_boxes_and_labels_on_image_array(
image,
np.squeeze(boxes),
np.squeeze(classes).astype(np.int32),
np.squeeze(scores),
category_index,
use_normalized_coordinates=True,
line_thickness=2)
objArray.detections =[]
objArray.header=data.header
object_count=1
for i in range(len(objects)):
object_count+=1
objArray.detections.append(self.object_predict(objects[i],data.header,image_np,cv_image))
self.object_pub.publish(objArray)
img=cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB)
image_out = Image()
try:
image_out = self.bridge.cv2_to_imgmsg(img,"bgr8")
except CvBridgeError as e:
print(e)
image_out.header = data.header
self.image_pub.publish(image_out)
def object_predict(self,object_data, header, image_np,image):
image_height,image_width,channels = image.shape
obj=Detection2D()
obj_hypothesis= ObjectHypothesisWithPose()
object_id=object_data[0]
object_score=object_data[1]
dimensions=object_data[2]
obj.header=header
obj_hypothesis.id = object_id
obj_hypothesis.score = object_score
obj.results.append(obj_hypothesis)
obj.bbox.size_y = int((dimensions[2]-dimensions[0])*image_height)
obj.bbox.size_x = int((dimensions[3]-dimensions[1] )*image_width)
obj.bbox.center.x = int((dimensions[1]+0.5*(dimensions[3]-dimensions[1]))*image_width)
obj.bbox.center.y = int((dimensions[0]+0.5*(dimensions[2]-dimensions[0]))*image_height)
return obj
class MiDaSROS:
def __init__(self):
'''Initialize ros publisher, ros subscriber'''
# topic where we publish
self.bridge = CvBridge()
self.image_depth_pub = rospy.Publisher("/midas/depth/image_raw",
Image,
queue_size=1)
self.image_rgb_pub = rospy.Publisher("/midas/rgb/image_raw",
Image,
queue_size=1)
self.camera_info_pub = rospy.Publisher("/midas/camera_info",
CameraInfo,
queue_size=1)
# subscribed Topic
self.subscriber = rospy.Subscriber("/midas_rgb/image_raw",
Image,
self.callback,
queue_size=1)
# setup image display
self.display_rgb = False
self.display_depth = True
# initialize Intel MiDas
self.initialized_midas = False
rospack = rospkg.RosPack()
ros_pkg_path = rospack.get_path('intelisl_midas_ros')
model_path = os.path.join(ros_pkg_path, 'src/model-f6b98070.pt')
self.model = MidasNet(model_path, non_negative=True)
self.device = torch.device(
"cuda") if torch.cuda.is_available() else torch.device("cpu")
self.model.to(self.device)
self.model.eval()
rospy.loginfo('Loaded Intel MiDaS')
midas_transforms = torch.hub.load("intel-isl/MiDaS", "transforms")
self.transform = midas_transforms.default_transform
rospy.loginfo('Initialized Intel MiDaS transform')
self.initialized_midas = True
def show_image(self, img, window_name="Image Window"):
cv2.namedWindow(window_name, cv2.WINDOW_NORMAL)
cv2.imshow(window_name, img)
cv2.waitKey(2)
def callback(self, img_msg):
# conversion to OpenCV and the correct color
img = cv2.cvtColor(
self.bridge.imgmsg_to_cv2(img_msg, desired_encoding='passthrough'),
cv2.COLOR_BGR2RGB)
if self.display_rgb:
self.show_image(img, window_name='Ground Truth RGB')
# convert RGB to depth using MiDaS
if self.initialized_midas:
input_batch = self.transform(img).to(self.device)
with torch.no_grad():
prediction = self.model(input_batch)
prediction = torch.nn.functional.interpolate(
prediction.unsqueeze(1),
size=img.shape[:2],
mode="bicubic",
align_corners=False,
).squeeze()
# scale pixel values to display
omax, omin = prediction.max(), prediction.min()
prediction = (prediction - omin) / (omax - omin)
# convert depth prediction to numpy
output = prediction.cpu().numpy()
if self.display_depth:
self.show_image(output, window_name='Estimated Depth')
# setup message (depth)
depth_msg = self.bridge.cv2_to_imgmsg(output,
encoding="passthrough")
# setup message camera info
camera_info_msg = CameraInfo()
camera_info_msg.header.stamp = img_msg.header.stamp
camera_info_msg.height = img.shape[0]
camera_info_msg.width = img.shape[1]
# publish
self.image_depth_pub.publish(depth_msg)
self.image_rgb_pub.publish(img_msg)
self.camera_info_pub.publish(camera_info_msg)
class image_converter:
global lines
def __init__(self):
self.start = 0
self.pitch = 0
self.yaw = 0
self.po_dao = 0
self.count = 0
self.last_error = 0
self.pd_ping_flag = 0
self.last_rr = np.zeros((179, 1))
self.last_ll = np.zeros((179, 1))
self.bmx_flag = 0
self.last_img = np.empty([180, 320])
self.image_pub = rospy.Publisher("image_topic_2", Image)
self.pub = rospy.Publisher('/cv/error', String) #发布话题
self.sub_imu = rospy.Subscriber('/imu_data', Imu, self.imu_callback)
self.bridge = CvBridge()
self.a = cv2.imread('./100.png')
self.h, self.w = self.a.shape[:2]
self.pts = np.float32([[50, 100], [263, 100], [0, 180], [319, 180]])
self.pts1 = np.float32([[0, 0], [self.w - 1, 0], [0, self.h - 1],
[self.w - 1, self.h - 1]])
self.m = cv2.getPerspectiveTransform(self.pts, self.pts1)
# self.image_sub = rospy.Subscriber("/usb_cam/image_raw",Image,self.callback)
self.image_sub = rospy.Subscriber("/cv_camera/image_raw", Image,
self.callback)
def clean_flag(self):
self.bmx_flag = 0
self.po_dao = 0
self.pd_ping_flag = 0
def rgb_hls_lab(self, img):
img_lab = cv2.cvtColor(img, cv2.COLOR_BGR2LAB) #提取黄色 b通道
img_hls = cv2.cvtColor(img, cv2.COLOR_BGR2HLS) #提取白色 l通道
imghls_h = img_hls[:, :, 0]
imghls_l = img_hls[:, :, 1]
imghls_s = img_hls[:, :, 2]
#gray = cv2.equalizeHist(gray)
imglab_l = img_lab[:, :, 0]
imglab_a = img_lab[:, :, 1]
imglab_b = img_lab[:, :, 2]
retval_lab, dst_lab = cv2.threshold(imglab_b, 170, 255,
cv2.THRESH_BINARY)
retval_hls, dst_hls = cv2.threshold(imghls_l, 180, 255,
cv2.THRESH_BINARY)
# imglab_b = cv2.equalizeHist(imglab_b)
# imghls_l = cv2.equalizeHist(imghls_l)
#hls_lab = np.zeros_like(dst_lab)
# hls_lab = dst_lab + dst_hls
# cv2.imshow('b',dst_lab)
# cv2.imshow('l',dst_hls)
#cv2.imshow('b',imglab_b)
print(dst_hls.shape[0])
print(dst_hls.shape[1])
count = np.sum(dst_hls) / 255
print('white_count', count)
cv2.imshow('l', imghls_l)
cv2.imshow('l_', dst_hls)
# if count > 17000 and self.bmx_flag == 0:
# self.bmx_flag = 1
# t = Timer(2.0,self.clean_flag)
# t.start()
#cv2.imshow('hls_lab',hls_lab)
return count
def extract(self, img): #找到距离中心线最近的4个轮廓
img_height = 180
img_width = 320
blur = cv2.GaussianBlur(img, (3, 3), 0, 0)
gray = cv2.cvtColor(blur, cv2.COLOR_BGR2GRAY)
gray_cp = gray[59:179, :]
mid = np.median(gray_cp)
min_thresh = (int)(mid * 0.66)
max_thresh = (int)(mid * 1.33)
#gray = cv2.equalizeHist(gray)
#edges = cv2.Canny(gray,50,200)
edges = cv2.Canny(gray, min_thresh, max_thresh)
temp = np.ones(edges.shape, np.uint8) * 255 #白色幕布
cv2.rectangle(edges, (0, 0), (319, 60), 0, -1)
cv2.imshow('qiege', edges)
h = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
contours = h[1]
contours = [
contour for contour in contours
if ((abs)(cv2.fitLine(contour, cv2.DIST_L2, 0.2, 0.01, 0.01)[1] /
cv2.fitLine(contour, cv2.DIST_L2, 0.2, 0.01, 0.01)[0]) >
0.45 and len(contour) > 100)
]
contours_bmx = [contour for contour in contours if len(contour) > 200]
print('wai', len(contours))
if self.pitch > 25 and len(contours) >= 12 and self.pd_ping_flag == 0:
self.pd_ping_flag = 1
t = Timer(2.0, self.clean_flag)
t.start()
print('bmx', len(contours_bmx))
if (len(contours_bmx) >= 9 and self.bmx_flag == 0 and self.pitch < 8):
self.bmx_flag = 1
t = Timer(2.0, self.clean_flag)
t.start()
cv2.drawContours(temp, contours, -1, (0, 255, 0), cv2.FILLED)
cv2.imshow('contours', temp)
return contours, temp
def imu_data_deal(self):
error = 0
percent = (abs)(self.yaw) / 360
yaw = abs(self.yaw)
if (percent <= 0.125 or percent > 0.875):
if (percent <= 0.125):
error = yaw - 0
elif (percent > 0.875):
error = yaw - 360
elif (percent > 0.125 and percent <= 0.375):
error = yaw - 90
elif (percent > 0.375 and percent <= 0.625):
error = yaw - 180
elif (percent > 0.625 and percent <= 0.875):
error = yaw - 270
return error
def transform(self, img, transform_param): #透视变换
dst = cv2.warpPerspective(img, transform_param, (320, 180))
cv2.imshow('dst', dst)
return dst
def counts_select(self, contours, img_ed, img): #中线提取
img_cp = np.copy(img_ed) #副本图像
img_height = 180
img_width = 320
point_list_l = []
point_list_r = []
contour_position = 'l'
ll_q = 0
rr_q = 0
# print ('start',len(contours))
point_listr = []
point_listr_index = []
point_listl = []
point_listl_index = []
count_l = 0
count_r = 0
for contour in contours:
rows, cols = img_cp.shape[:2]
[vx, vy, x_, y_] = cv2.fitLine(contour, cv2.DIST_L2, 0.2, 0.01,
0.01)
pose_x, pose_y, pose_w, pose_h = cv2.boundingRect(contour)
if (pose_w >= 25 and vy / vx < 0):
count_l = count_l + 1
elif (pose_w >= 25 and vy / vx > 0):
count_r = count_r + 1
if (pose_w >= 50 and vy / vx < 0) or (
pose_w < 50 and
(pose_x + pose_w + pose_x) / 2 <= img_width / 2):
contour_position = 'l'
ll_q = ll_q + pose_w
point_listl_index.append(pose_x)
point_listl.append(contour)
if (pose_w >= 50 and vy / vx > 0) or (
pose_w < 50 and
(pose_x + pose_w + pose_x) / 2 >= img_width / 2):
contour_position = 'r'
rr_q = rr_q + pose_w
point_listr_index.append(pose_x)
point_listr.append(contour)
else:
continue
print('l,r', count_l, count_r)
find_countl = 0
find_countr = 0
if len(contours) <= 1:
find_countl = len(contours)
find_countr = len(contours)
else:
find_countr = 1
find_countl = 1
dis_minr = (heapq.nsmallest(find_countr, point_listr_index))
dis_minl = (heapq.nlargest(find_countl, point_listl_index))
deal_contoursl = []
deal_contoursr = []
for i in dis_minl:
index = point_listl_index.index(i)
deal_contoursl.append(point_listl[index])
for i in dis_minr:
index = point_listr_index.index(i)
deal_contoursr.append(point_listr[index])
if (ll_q - rr_q >= 100 or count_l - count_r >= 2):
for contour in deal_contoursl:
for pose in contour:
x = pose[0][0]
y = pose[0][1]
point = (x, y)
if y >= (int)(img_height / 5):
point_list_l.append(point)
elif (rr_q - ll_q >= 100 or count_r - count_l >= 2):
for contour in deal_contoursr:
#[vxl,vyl,xl_,yl_] = cv2.fitLine(np.array(contour),cv2.DIST_L2,0.2,0.01,0.01)
for pose in contour:
x = pose[0][0]
y = pose[0][1]
point = (x, y)
if y >= (int)(img_height / 5):
point_list_r.append(point)
elif (abs)(rr_q - ll_q) < 100:
for contour in point_listl:
for pose in contour:
x = pose[0][0]
y = pose[0][1]
point = (x, y)
if y >= (int)(img_height / 5):
point_list_l.append(point)
for contour in point_listr:
for pose in contour:
x = pose[0][0]
y = pose[0][1]
point = (x, y)
if y >= (int)(img_height / 5):
point_list_r.append(point)
# print ('end')
print('ll_q', ll_q)
print('rr_q', rr_q)
npl = np.mat(point_list_l)
npr = np.mat(point_list_r)
ll_ = np.zeros((img_height, 1))
rr_ = np.full((img_height, 1), img_width - 1)
width = np.full((img_height, 1), 0)
for i in range(img_height):
l = 40
r = 280
wid = r - l
width[i] = wid
#把轮廓上杂乱的点 提取成左右线（一行只有一个点）
for pose in npl:
pose = pose.A #必须要将矩阵转化为数组
if (pose.shape[1] != 2):
continue
x = pose[0][0]
y = pose[0][1]
if (y > img_height - 120):
if (ll_[y] == 0):
ll_[y] = x
else:
if ll_[y] > x:
ll_[y] = ll_[y]
else:
ll_[y] = x
for pose in npr:
pose = pose.A #必须要将矩阵转化为数组
if (pose.shape[1] != 2):
continue
x = pose[0][0]
y = pose[0][1]
if (y > img_height - 120):
if (rr_[y] == img_width - 1):
rr_[y] = x
else:
if rr_[y] < x:
rr_[y] = rr_[y]
else:
rr_[y] = x
point_deal_l = []
point_deal_r = []
for index, i in enumerate(ll_):
if (index <= 60):
continue
if (i[0] == 0):
if (rr_[index] == img_width - 1):
i[0] = 25
ll_[index] = 25
else:
i[0] = rr_[index] - width[index]
ll_[index] = rr_[index] - width[index]
pose = (int(i[0]), int(index))
point_deal_l.append(pose)
for index, i in enumerate(rr_):
if (index <= 60):
continue
if (i[0] == img_width - 1):
if (ll_[index] == 0):
i[0] = 295
rr_[index] = 295
else:
i[0] = ll_[index] + width[index]
rr_[index] = ll_[index] + width[index]
pose = (int(i[0]), int(index))
point_deal_r.append(pose)
[vxl, vyl, xl_, yl_] = cv2.fitLine(np.array(point_deal_l), cv2.DIST_L2,
0.2, 0.01, 0.01)
[vxr, vyr, xr_, yr_] = cv2.fitLine(np.array(point_deal_r), cv2.DIST_L2,
0.2, 0.01, 0.01)
kl = vyl / vxl
kr = vyr / vxr
for y, i in enumerate(ll_):
if y < 60:
continue
if ((rr_[y] < ll_[y]) or ((abs)(rr_[y] - ll_[y]) < 40)):
if (rr_q - ll_q > 125):
ll_[y] = rr_[y] - width[y]
elif (ll_q - rr_q > 125):
rr_[y] = ll_[y] + width[y]
if (kl > 1 and kr > 1):
ll_[y] = rr_[y] - width[y]
elif (kr < -1 and kl < -1):
rr_[y] = ll_[y] + width[y]
print('kl', vyl / vxl)
print('kr', vyr / vxr)
print(len(ll_), len(self.last_ll))
for y in range(179):
if (self.start == 0):
self.start = 1
break
if y < 60:
continue
if (abs)(
self.last_ll[y] - ll_[y]
) > 50 and self.bmx_flag == 0 and self.po_dao == 0 and self.pd_ping_flag == 0:
ll_[y] = self.last_ll[y]
if (abs)(
self.last_rr[y] - rr_[y]
) > 50 and self.bmx_flag == 0 and self.po_dao == 0 and self.pd_ping_flag == 0:
rr_[y] = self.last_rr[y]
if self.bmx_flag == 1 or self.pd_ping_flag == 1:
rr_ = np.full((img_height, 1), 280)
ll_ = np.full((img_height, 1), 40)
for index, i in enumerate(rr_):
pose = ((int)(i[0]), int(index))
cv2.circle(img, pose, 1, (255, 0, 0), 1)
for index, i in enumerate(ll_):
pose = (int(i[0]), int(index))
cv2.circle(img, pose, 1, (255, 0, 255), 1)
for index, i in enumerate(self.last_rr):
pose = ((int)(i[0]), int(index))
cv2.circle(img, pose, 1, (100, 0, 0), 1)
for index, i in enumerate(self.last_ll):
pose = (int(i[0]), int(index))
cv2.circle(img, pose, 1, (255, 0, 100), 1)
self.last_rr = rr_.copy()
self.last_ll = ll_.copy()
#得到中线
mid = []
for i in range(img_height):
pose = ((int)((ll_[i] + rr_[i]) / 2), i)
mid.append(pose)
#出中线
for dian in mid:
cv2.circle(img, dian, 1, (100, 0, 255), 1)
#画出基准线
reference_line = []
actual_line = []
mid_value = 0
start_raw = 80
# print ('pitch:')
# print (self.pitch)
if ((self.pitch > 8 or self.pitch < -2) and self.po_dao == 0):
self.po_dao = 1
start_raw = 80
t = Timer(2.0, self.clean_flag)
t.start()
self.pitch = float(self.pitch)
self.yaw = round(self.yaw, 2)
text_pitch = str(self.pitch)
text_yaw = str(self.yaw)
bmx_flag = str(self.bmx_flag)
text_raw = str(start_raw)
text_pd = str(self.po_dao)
cv2.putText(img, text_pitch, (250, 50), cv2.FONT_HERSHEY_COMPLEX, 1.0,
(255, 255, 200), 2)
cv2.putText(img, bmx_flag, (50, 100), cv2.FONT_HERSHEY_COMPLEX, 1.0,
(255, 255, 200), 2)
cv2.putText(img, text_pd, (100, 100), cv2.FONT_HERSHEY_COMPLEX, 1.0,
(255, 255, 200), 2)
cv2.putText(img, text_yaw, (0, 50), cv2.FONT_HERSHEY_COMPLEX, 1.0,
(255, 0, 200), 2)
cv2.putText(img, text_raw, (250, 100), cv2.FONT_HERSHEY_COMPLEX, 1.0,
(255, 255, 200), 2)
for i in range(10): #计算60行的中值 求平均
mid_value = mid_value + mid[start_raw + i][0]
pose = (img_width / 2, start_raw + i)
reference_line.append(pose)
mid_value = (int)(mid_value / 10)
print('mid_value', mid_value)
for i in range(15):
pose = (mid_value, 75 + i)
actual_line.append(pose)
for dian in reference_line:
cv2.circle(img, dian, 1, (0, 255, 0), 1)
for dian in actual_line:
cv2.circle(img, dian, 1, (150, 255, 0), 1)
error = mid_value - (img_width / 2) + (ll_q - rr_q) * 0.1
if (self.bmx_flag == 1 or self.pd_ping_flag == 1):
error = -self.imu_data_deal() * 10
# if (abs)(error - self.last_error) > 150:
# error = self.last_error
#pass
# self.last_error = error
error = float(error)
text_error = str(error)
cv2.putText(img, text_error, (100, 50), cv2.FONT_HERSHEY_COMPLEX, 1.0,
(255, 255, 200), 5)
cv2.imshow("final", img)
return error
def imu_callback(self, data):
self.pitch = data.orientation.y
self.yaw = data.orientation.z
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
start = time.time()
img_cp = np.copy(cv_image)
cv2.imshow('one', cv_image)
transform_img = self.transform(cv_image, self.m) #透视变换后的图像
img_hl = self.rgb_hls_lab(transform_img)
contours, edges = self.extract(transform_img) #边缘提取后的轮廓和图像
print(edges.shape)
print(self.last_img.shape)
cha = np.subtract(edges, self.last_img)
cv2.imshow('img_cha', cha)
#contours,edges = self.extract(bgr_img) #边缘提取后的轮廓和图像
self.last_img = np.copy(edges)
cv2.imshow('img___', self.last_img)
error = self.counts_select(contours, edges,
transform_img) #中线提取 最终得到偏差值
#error = 0
end = time.time()
print('cost time', end - start)
count = 0
if self.bmx_flag == 1:
str_zerba = 1
else:
str_zerba = 0
str_error = "%d" % (error) #将数字转换为字符串
str_msg = str_error + " " + str(str_zerba)
self.pub.publish(str_msg) #发布偏差话题
cv2.waitKey(3)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
def stereo_callback(self, data):
bridge = CvBridge()
global cv_image
cv_image = bridge.imgmsg_to_cv2(data, "mono8")
class ObjectDetectionNode:
def __init__(self):
self.camera_topic = rospy.get_param("/object_detection_node/camera_topic")
self.obj_detect_results_topic = rospy.get_param("/object_detection_node/obj_detect_results_topic")
self.obj_detect_viz_topic = rospy.get_param("/object_detection_node/obj_detect_viz_topic")
rospy.init_node('object_detection')
self.camera_sub = rospy.Subscriber(self.camera_topic,
Image, self.image_update_callback, queue_size=1)
self.current_frame = None
self.bridge = CvBridge()
rospy.loginfo("Object Detection Initializing")
rospy.loginfo("Tiny Yolo V3")
self.model_path = rospy.get_param("/object_detection_node/model_path")
self.classes_path = rospy.get_param("/object_detection_node/classes_path")
self.anchors_path = rospy.get_param("/object_detection_node/anchors_path")
self.iou_threshold = rospy.get_param("/object_detection_node/iou_threshold")
self.score_threshold = rospy.get_param("/object_detection_node/score_threshold")
self.input_size = (416, 416)
self.detector = ObjectDetector(model_path=self.model_path,
classes_path=self.classes_path,
anchors_path=self.anchors_path,
score_threshold=self.score_threshold,
iou_threshold=self.iou_threshold,
size=self.input_size)
labels_visualizer = utils.LabelsVisualizer(self.detector.class_names)
detection_image_pub = rospy.Publisher(self.obj_detect_viz_topic + "/compressed",
CompressedImage, queue_size=1)
detection_results_pub = rospy.Publisher(self.obj_detect_results_topic,
DetectionResults, queue_size=1)
rate = rospy.Rate(15)
while not rospy.is_shutdown():
time = rospy.get_time()
if self.current_frame is not None:
print("new image at time %.2f" % time)
out_boxes, out_scores, out_classes = \
self.detector.detect_object(self.current_frame)
inference_end_time = rospy.get_time()
print("*** inference time: %.3f" % (inference_end_time - time))
if detection_image_pub.get_num_connections() > 0:
image = labels_visualizer.draw_bboxes(self.current_frame,
out_boxes, out_scores, out_classes)
print("*** drawing time: %.3f" % (rospy.get_time() - inference_end_time))
# img_msg = self.bridge.cv2_to_imgmsg(image, "bgr8")
msg = utils.cv2_to_compressed_imgmsg(image)
detection_image_pub.publish(msg)
msg = self.convert_results_to_message(out_boxes, out_scores, out_classes)
msg.is_green = self.check_traffic_segmentation(out_boxes, out_classes)
if detection_results_pub.get_num_connections() > 0:
detection_results_pub.publish(msg)
else:
print("waiting for image at time %.2f" % time)
rate.sleep()
def check_traffic_segmentation(self, out_boxes, out_classes):
labels = [self.detector.class_names[i] for i in out_classes]
if 'TRAFFIC_LIGHT' not in labels:
return True
ind = labels.index('TRAFFIC_LIGHT')
bbox = out_boxes[ind]
top, left, bottom, right = bbox
top = max(0, np.floor(top + 0.5).astype('int32'))
left = max(0, np.floor(left + 0.5).astype('int32'))
bottom = min(image.size[1], np.floor(bottom + 0.5).astype('int32'))
right = min(image.size[0], np.floor(right + 0.5).astype('int32'))
bbox = ((left, top), (right, bottom))
print '************************** BBOX', bbox
'''if (x2 - x1) * (y2 - y1) < 13500:
return True'''
return not check_traffic_light(self.current_frame, bbox)['detected_traffic_stop']
def image_update_callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
raise e
# cv_image = utils.compressed_imgmsg_to_cv2(data)
self.current_frame = cv_image
@staticmethod
def convert_results_to_message(out_boxes, out_scores, out_classes):
msgs = DetectionResults()
for i in range(len(out_scores)):
msg = DetectionResult()
msg.out_class = out_classes[i]
msg.out_score = out_scores[i]
msg.location = out_boxes[i, :]
msgs.results.append(msg)
return msgs
class gcsrvclient:
def __init__(self):
self.bridge = CvBridge()
# set ros publishers and subscribers
self.image_pub = rospy.Publisher("image_topic", Image, queue_size=2)
self.result_pub = rospy.Publisher("result_topic", String, queue_size=2)
self.image_sub = rospy.Subscriber("/usb_cam/image_raw",
Image,
self.imageCallback,
queue_size=2)
# main image variables
self.image_msg = False
self.cv_image = False
self.current_image = False
# parameters
self.display_image = True
self.display_labels = True
self.update_rate = 1.0
# control variables
self.have_image = False
self.wait_window = True
self.have_detection = False
# set next image update for now
self.next_time = rospy.get_time()
print "init done"
def get_image(self):
# convert to ros image to cv image
try:
cv_image = self.bridge.imgmsg_to_cv2(self.current_image, "bgr8")
except CvBridgeError as e:
print(e)
# set image and control variables
image_message = self.current_image
self.have_image = False
return image_message, cv_image
def get_emos(self, f):
emos = {k: v for k, v in f.items() if k.endswith('Likelihood')}
emos = {k: v for k, v in emos.items() if v != 'VERY_UNLIKELY'}
return emos
def draw_emos(self, image, f, pos):
emos = self.get_emos(f)
if emos:
for k, v in emos.items():
t = "%s : %s" % (k, v)
x = pos[0]
y = pos[1]
cv2.putText(image, t, (x, y), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(0, 255, 0), 2)
def show_detection(self, image, response):
resp = eval(response)
#result = resp['responses']
result = resp['responses'][0]
if not 'faceAnnotations' in result:
print "No Face 0"
return
for f in result['faceAnnotations']:
box = [(v['x'], v['y']) for v in f['fdBoundingPoly']['vertices']]
pts = numpy.array(box, numpy.int32)
pts = pts.reshape((-1, 1, 2))
cv2.polylines(image, [pts], True, (0, 255, 0), thickness=2)
self.draw_emos(image, f, box[1])
def print_labels(self, response):
resp = eval(response)
result = resp['responses'][0]
print json.dumps(result, indent=2, sort_keys=True)
def create_service_request(self, img):
req = gc_srvs.RequestAnnotationsRequest()
req.image = img
req.annotations = ['FACE_DETECTION']
req.max_results = [10]
return req
def imageCallback(self, data):
# filter image for desired rate
if rospy.get_time() < self.next_time:
return
self.next_time = rospy.get_time() + 1.0
# set image and control variables
self.current_image = data
self.have_image = True
def publish_results(self, response):
resp = eval(response)
result = resp['responses'][0]
self.image_pub.publish(self.image_msg)
self.result_pub.publish(json.dumps(result, indent=2, sort_keys=True))
def spin(self):
if self.have_image:
# set image for request
self.image_msg, self.cv_image = self.get_image()
# request an get results
req = self.create_service_request(self.image_msg)
GetAnnotationSrv = rospy.ServiceProxy('get_annotation',
gc_srvs.RequestAnnotations)
resp = GetAnnotationSrv(req)
# output results
self.print_labels(resp.response)
self.show_detection(self.cv_image, resp.response)
self.publish_results(resp.response)
# show debug window
if self.display_image:
cv2.imshow('modified', self.cv_image)
if self.wait_window:
cv2.waitKey(2000)
else:
cv2.waitKey(1)
class task1_2(object):
def __init__(self):
self.predict_ser = rospy.Service("prediction", task1out,
self.prediction_cb)
self.cv_bridge = CvBridge()
# cropped version from https://github.com/pytorch/vision/blob/master/torchvision/models/vgg.py
self.cfg = {
'vgg11':
[64, 'M', 128, 'M', 256, 256, 'M', 512, 512, 'M', 512, 512, 'M'],
'vgg13': [
64, 64, 'M', 128, 128, 'M', 256, 256, 'M', 512, 512, 'M', 512,
512, 'M'
],
'vgg16': [
64, 64, 'M', 128, 128, 'M', 256, 256, 256, 'M', 512, 512, 512,
'M', 512, 512, 512, 'M'
],
'vgg19': [
64, 64, 'M', 128, 128, 'M', 256, 256, 256, 256, 'M', 512, 512,
512, 512, 'M', 512, 512, 512, 512, 'M'
],
}
self.means = np.array([
103.939, 116.779, 123.68
]) / 255. # mean of three channels in the order of BGR
self.h, self.w = 480, 640
self.n_class = 4
model_dir = "/root/sis_mini_competition_2018" #
model_name = "sis_99epoch.pkl"
self.vgg_model = VGGNet(self.cfg, requires_grad=True, remove_fc=True)
self.fcn_model = FCN16s(pretrained_net=self.vgg_model,
n_class=self.n_class)
use_gpu = torch.cuda.is_available()
num_gpu = list(range(torch.cuda.device_count()))
rospy.loginfo("Cuda available: %s", use_gpu)
if use_gpu:
ts = time.time()
self.vgg_model = self.vgg_model.cuda()
self.fcn_model = self.fcn_model.cuda()
self.fcn_model = nn.DataParallel(self.fcn_model,
device_ids=num_gpu)
print("Finish cuda loading, time elapsed {}".format(time.time() -
ts))
state_dict = torch.load(os.path.join(model_dir, model_name))
self.fcn_model.load_state_dict(state_dict)
self.mask1 = np.zeros((self.h, self.w))
self.MAXAREA = 18000
self.MINAREA = 1000
self.brand = ['doublemint', 'kinder', 'kusan']
rospy.loginfo("Service ready!")
def prediction_cb(self, req):
resp = task1outResponse()
im_msg = rospy.wait_for_message('/camera/rgb/image_rect_color',
Image,
timeout=None)
resp.pc = rospy.wait_for_message('/camera/depth_registered/points',
PointCloud2,
timeout=None)
rospy.loginfo("Get image.")
resp.org_image = im_msg
try:
img = self.cv_bridge.imgmsg_to_cv2(im_msg, "bgr8")
except CvBridgeError as e:
print(e)
origin = img
img = img[:, :, ::-1] # switch to BGR
img = np.transpose(img, (2, 0, 1)) / 255.
img[0] -= self.means[0]
img[1] -= self.means[1]
img[2] -= self.means[2]
now = rospy.get_time()
# convert to tensor
img = img[np.newaxis, :]
img = torch.from_numpy(img.copy()).float()
output = self.fcn_model(img)
output = output.data.cpu().numpy()
N, _, h, w = output.shape
mask = output.transpose(0, 2, 3, 1).reshape(
-1, self.n_class).argmax(axis=1).reshape(N, h, w)[0]
rospy.loginfo("Predict time : %f", rospy.get_time() - now)
now = rospy.get_time()
show_img = np.asarray(origin)
count = np.zeros(3)
self.mask1[:, :] = 0
self.mask1[mask != 0] = 1
labels = self.adj(self.mask1)
mask = np.asarray(mask, np.uint8)
mask2 = np.zeros((h, w))
rospy.loginfo("f**k 1")
for i in range(1, self.n_class):
self.mask1[:, :] = 0
self.mask1[mask == i] = 1
self.mask1 = np.asarray(self.mask1, np.uint8)
self.mask1 = cv2.GaussianBlur(self.mask1, (5, 5), 0)
cnts = cv2.findContours(self.mask1.copy(), cv2.RETR_CCOMP,
cv2.CHAIN_APPROX_SIMPLE)
cnts = cnts[1]
sd = ShapeDetector()
rospy.loginfo("f**k 2")
for c in cnts:
if self.MAXAREA >= cv2.contourArea(c) >= self.MINAREA:
rospy.loginfo("f**k 3")
shape = sd.detect(c)
if shape is "rectangle":
M = cv2.moments(c)
if M["m00"] == 0:
break
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
ry = float(480 - cY) / 600
rx = float(cX - 320) * (48 + 0.675 * ry) / 64000
rospy.loginfo("(%f,%f)", rx, ry)
#######Service for arm########
rospy.wait_for_service('arm_planning')
try:
move_arm = rospy.ServiceProxy(
'arm_planning', Move_Arm)
move_arm(ry + 0.03, -rx) #0.141,0.02
except rospy.ServiceException, e:
print "Service call failed: %s" % e
##############################
rect = cv2.minAreaRect(c)
box = cv2.boxPoints(rect)
box = np.int0(box)
cv2.drawContours(show_img, [box], 0, (255, 0, 0), 2)
cv2.putText(show_img, self.brand[i - 1], (cX, cY),
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 0),
2)
#_, labels = cv2.conectedComponents(mask)
p = labels[c[0, 0, 1], c[0, 0, 0]]
mask2[labels == p] = i
count[i - 1] += 1
rospy.loginfo("Image processing time : %f", rospy.get_time() - now)
#cv2.imshow('My image', show_img)
cv2.imwrite("/hosthome/test.jpg", show_img)
resp.process_image = self.cv_bridge.cv2_to_imgmsg(show_img, "bgr8")
resp.mask = self.cv_bridge.cv2_to_imgmsg(mask2, "64FC1")
print(count)
return resp
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.base_waypoints = None
self.camera_image = None
self.lights = []
self.stopline_waypoints = []
self.has_image = False
# We thank John Chen's mentioning of this apprach in the slack channel #s_p-system-integrat
self.path = rospy.get_param('~model_path')
self.camera_topic = rospy.get_param('~camera_topic')
####
self.sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
self.sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
'''
/vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and
helps you acquire an accurate ground truth data source for the traffic light
classifier by sending the current color state of all traffic lights in the
simulator. When testing on the vehicle, the color state will not be available. You'll need to
rely on the position of the light and the camera image to predict it.
'''
self.sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_cb)
self.sub6 = rospy.Subscriber(self.camera_topic, Image, self.image_cb)
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
self.upcoming_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1)
self.bridge = CvBridge()
self.light_classifier = TLClassifier(self.path)
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
# constrain the rate at which this node is processed to specified rate; ros::spin() would add unnecessary
# overhead given this node's purpose (see below)
# https://stackoverflow.com/questions/40508651/writing-a-ros-node-with-both-a-publisher-and-subscriber
self.updateRate = 10
self.loop()
def loop(self):
rate = rospy.Rate(self.updateRate)
while not rospy.is_shutdown():
# if following condtions met all required subcription callback functions have run
if self.pose and self.base_waypoints and self.has_image:
light_wp, state = self.process_traffic_lights()
'''
Publish upcoming red lights at camera frequency.
Each predicted state has to occur `STATE_COUNT_THRESHOLD` number
of times till we start using it. Otherwise the previous stable state is
used.
'''
if self.state != state:
self.state_count = 0
self.state = state
elif self.state_count >= STATE_COUNT_THRESHOLD:
# distinguish among cases (publish (-) values if GREEN or UNKNOWN)
if state == TrafficLight.GREEN or state == TrafficLight.UNKNOWN:
light_wp = -1
self.last_wp = light_wp
self.upcoming_light_pub.publish(Int32(light_wp))
# for debug
print ("tl_detector state: ", Int32(light_wp))
print ("tl_detector state: ", state)
print ("tl_detector state: ", Int32(light_wp))
print ("tl_detector state: ", state)
else:
self.upcoming_light_pub.publish(Int32(self.last_wp))
self.state_count += 1
rate.sleep()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, msg):
if self.base_waypoints is None:
self.base_waypoints = []
for wp in msg.waypoints:
self.base_waypoints.append(wp)
# only do this once since these values don't change
self.sub2.unregister()
rospy.loginfo('base_waypoints sub unregistered')
# initilize waypoints at which the car must stop if the light is red
self.initialize_stop_positions()
def traffic_cb(self, msg):
self.lights = msg.lights
def image_cb(self, msg):
""" Interface with ROS to receive image
Args:
msg (Image): image from car-mounted camera
"""
rospy.logwarn('image received by tl_detector node')
self.has_image = True
self.camera_image = msg
# Used for stop position waypoints
def create_pose_vector(self, x, y, z):
wp = PoseStamped()
wp.pose.position.x = x
wp.pose.position.y = y
wp.pose.position.z = z
return wp
def initialize_stop_positions(self):
# Returns waypoint indexes for each stop position
# List of positions that correspond to the line to stop in front of at a given intersection
stop_line_positions = self.config['stop_line_positions']
for i in range(len(stop_line_positions)):
temp = self.create_pose_vector(stop_line_positions[i][0], stop_line_positions[i][1], 0)
index = self.get_closest_waypoint(temp.pose, self.base_waypoints, 1e10)
self.stopline_waypoints.append(index)
def get_pose_vector(self, pose):
return np.asarray([pose.position.x, pose.position.y, pose.position.z], np.float32)
def get_closest_waypoint(self, pose, waypoints, dist):
"""Identifies the index of the given position within waypoints if less than specified distance
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
pose (Pose): position to match a waypoint to
waypoints: ordered list of waypoints (either track waypoints or traffic lights waypoints)
dist: only for positions w/ distances less than this value
"""
# TODO implement
index = None
for i, loc in enumerate(waypoints):
a = self.get_pose_vector(pose)
b = self.get_pose_vector(loc.pose.pose)
temp = np.linalg.norm(a - b)
if dist > temp:
dist = temp
index = i
return index
def get_light_state(self):
"""Determines the current color of the traffic light
Args:
light (TrafficLight): light to classify
Returns:
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
if (not self.has_image):
return False
# We thank John Chen's mentioning of this apprach in the slack channel #s_p-system-integrat:
# fixing convoluted camera encoding...
if hasattr(self.camera_image, 'encoding'):
self.attribute = self.camera_image.encoding
if self.camera_image.encoding == '8UC3':
self.camera_image.encoding = "rgb8"
else:
self.camera_image.encoding = 'rgb8'
####
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8")
# Get classification
classification = self.light_classifier.get_classification(cv_image)
return classification
def process_traffic_lights(self):
"""Finds closest visible traffic light, if one exists, and determines its
location and color
Returns:
int: index of waypoint closes to the upcoming stop line for a traffic light (-1 if none exists)
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
# TODO find the closest visible traffic light (if one exists)
# ***only check for lights within 100 meters***#
tl_index = self.get_closest_waypoint(self.pose.pose, self.lights, 100)
if tl_index is not None:
light_wp = self.get_closest_waypoint(self.lights[tl_index].pose.pose, self.base_waypoints, 1e10)
waypoints_greater = [i for i, v in enumerate(self.stopline_waypoints) if v > light_wp]
if len(waypoints_greater) > 0:
first_greater_than = waypoints_greater[0]
else:
first_greater_than = len(self.stopline_waypoints)
state = self.get_light_state()
return self.stopline_waypoints[first_greater_than - 1], state
else:
return -1, TrafficLight.UNKNOWN
class dvrkGraspingDetection(threading.Thread):
def __init__(self, interval_ms=10, video_recording=False):
# data members
self.__bridge = CvBridge()
self.__img_raw_left = []
self.__img_raw_right = []
self.__actuator_current_measured_PSM1 = []
self.__actuator_current_measured_PSM2 = []
self.__state_jaw_current_PSM1 = []
self.__state_jaw_current_PSM2 = []
# threading
threading.Thread.__init__(self)
self.__interval_ms = interval_ms
self.__stop_flag = False
# subscriber
self.__sub_list = [
rospy.Subscriber('/endoscope/left/image_raw/compressed',
CompressedImage, self.__img_raw_left_cb),
rospy.Subscriber('/endoscope/right/image_raw/compressed',
CompressedImage, self.__img_raw_right_cb),
rospy.Subscriber('/dvrk/PSM1/io/actuator_current_measured',
JointState,
self.__actuator_current_measured_PSM1_cb),
rospy.Subscriber('/dvrk/PSM2/io/actuator_current_measured',
JointState,
self.__actuator_current_measured_PSM2_cb),
rospy.Subscriber('/dvrk/PSM1/state_jaw_current', JointState,
self.__state_jaw_current_PSM1_cb),
rospy.Subscriber('/dvrk/PSM2/state_jaw_current', JointState,
self.__state_jaw_current_PSM2_cb)
]
# create node
if not rospy.get_node_uri():
rospy.init_node('detect_grasping_node',
anonymous=True,
log_level=rospy.WARN)
self.rate = rospy.Rate(1000.0 / self.__interval_ms)
else:
rospy.logdebug(rospy.get_caller_id() +
' -> ROS already initialized')
# video recording
self.__video_recording = video_recording
if self.__video_recording is True:
fps = 1000.0 / self.__interval_ms
fcc = cv2.VideoWriter_fourcc('D', 'I', 'V', 'X')
self.__img_width = 640
self.__img_height = 360
self.out = cv2.VideoWriter('video_recorded.avi', fcc, fps,
(self.__img_width, self.__img_height))
print("start recording")
# initializing variables
self.__jaw1_curr_PSM1_prev = 0
self.__jaw2_curr_PSM1_prev = 0
self.__jaw1_curr_PSM2_prev = 0
self.__jaw2_curr_PSM2_prev = 0
self.start()
rospy.spin()
def start(self):
self.__stop_flag = False
self.__thread = threading.Thread(target=self.run,
args=(lambda: self.__stop_flag, ))
self.__thread.daemon = True
self.__thread.start()
def stop(self):
self.__stop_flag = True
def run(self, stop):
while True:
# Resizing images
img1 = cv2.resize(self.__img_raw_left,
(self.__img_width, self.__img_height))
img2 = cv2.resize(self.__img_raw_right,
(self.__img_width, self.__img_height))
# Low pass filtering
fc = 1 # (Hz)
dt = 0.01 # (ms)
jaw1_curr_PSM1 = U.LPF(self.__actuator_current_measured_PSM1[5],
self.__jaw1_curr_PSM1_prev, fc, dt)
self.__jaw1_curr_PSM1_prev = jaw1_curr_PSM1
jaw2_curr_PSM1 = U.LPF(self.__actuator_current_measured_PSM1[6],
self.__jaw2_curr_PSM1_prev, fc, dt)
self.__jaw2_curr_PSM1_prev = jaw2_curr_PSM1
jaw1_curr_PSM2 = U.LPF(self.__actuator_current_measured_PSM2[5],
self.__jaw1_curr_PSM2_prev, fc, dt)
self.__jaw1_curr_PSM2_prev = jaw1_curr_PSM2
jaw2_curr_PSM2 = U.LPF(self.__actuator_current_measured_PSM2[6],
self.__jaw2_curr_PSM2_prev, fc, dt)
self.__jaw2_curr_PSM2_prev = jaw2_curr_PSM2
# Current thresholding
self.__threshold_PSM1 = 0.3 # (A)
self.__threshold_PSM2 = 0.25 # (A)
if self.__state_jaw_current_PSM1[0] <= 0:
if jaw1_curr_PSM1 - jaw2_curr_PSM1 > self.__threshold_PSM1:
cv2.circle(img1, (540, 300), 30, (0, 255, 0), -1)
else:
cv2.circle(img1, (540, 300), 30, (0, 0, 255), -1)
else:
cv2.circle(img1, (540, 300), 30, (0, 0, 255), -1)
if self.__state_jaw_current_PSM2[0] <= 0:
if jaw1_curr_PSM2 - jaw2_curr_PSM2 > self.__threshold_PSM2:
cv2.circle(img1, (100, 300), 30, (0, 255, 0), -1)
else:
cv2.circle(img1, (100, 300), 30, (0, 0, 255), -1)
else:
cv2.circle(img1, (100, 300), 30, (0, 0, 255), -1)
cv2.imshow("original1", img1)
if self.__video_recording is True:
self.out.write(img1)
self.rate.sleep()
if cv2.waitKey(1) & 0xFF == ord('q'):
if self.__video_recording is True:
print("finishing recording")
self.out.release()
cv2.destroyAllWindows()
stop()
break
def __img_raw_left_cb(self, data):
try:
if type(data).__name__ == 'CompressedImage':
self.__img_raw_left = self.__compressedimg2cv2(data)
elif type(data).__name__ == 'Image':
self.__img_raw_left = self.__bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
def __img_raw_right_cb(self, data):
try:
if type(data).__name__ == 'CompressedImage':
self.__img_raw_right = self.__compressedimg2cv2(data)
elif type(data).__name__ == 'Image':
self.__img_raw_right = self.__bridge.imgmsg_to_cv2(
data, "bgr8")
except CvBridgeError as e:
print(e)
def __compressedimg2cv2(self, comp_data):
np_arr = np.fromstring(comp_data.data, np.uint8)
return cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
def __actuator_current_measured_PSM1_cb(self, data):
joint = np.array(0, dtype=np.float)
joint.resize(len(data.position))
joint.flat[:] = data.position
self.__actuator_current_measured_PSM1 = list(joint)
def __actuator_current_measured_PSM2_cb(self, data):
joint = np.array(0, dtype=np.float)
joint.resize(len(data.position))
joint.flat[:] = data.position
self.__actuator_current_measured_PSM2 = list(joint)
def __state_jaw_current_PSM1_cb(self, data):
joint = np.array(0, dtype=np.float)
joint.resize(len(data.position))
joint.flat[:] = data.position
self.__state_jaw_current_PSM1 = list(joint)
def __state_jaw_current_PSM2_cb(self, data):
joint = np.array(0, dtype=np.float)
joint.resize(len(data.position))
joint.flat[:] = data.position
self.__state_jaw_current_PSM2 = list(joint)
class following_final():
# Distance in mm
invalid_thresh = 300
desired_thresh = 1000
desired_lowBound = 950
desired_upBound = 1050
invalid_max = 2000
max_speed = 1
def __init__(self):
rospy.init_node('image_converter', anonymous=True)
# Initialize bridge
self.bridge = CvBridge()
# Subscribe to depth sensor and get raw image
self.depth_sub = rospy.Subscriber("/camera/depth/image_raw",Image,self.callback)
#self.depth_sub = rospy.Subscriber("/camera/depth_registered/image_raw",Image,self.callback_depth)
# Publish to navigation to move robot
self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
# Initialize Twist that will be published to cmd_vel
self.move_cmd = Twist()
self.r = rospy.Rate(10)
def callback(self,data):
try:
# Gain Values for movement
# X gain rotation
K = 0.0035
# Kx is for movment in z direction forward backwards
Kx = .0005
# Get Image and find size of image
self.depth_image = self.bridge.imgmsg_to_cv2(data, "passthrough")
rows, col, channels = self.depth_image.shape #grey scale channel is 1, rgb is 3
# Find Center of Image
cR = np.int(np.round(rows/2))
cC = np.int(np.round(col/2))
# How Large our field of view for our desired object i.e where we looking in the picture for our object
# Mask to remove any uneccesary information outside our looking area.
rowFrac = np.int(np.round(.25*cR))
colFrac = np.int(np.round(.4*cC))
#Masks the quadrant it is interested on (default = upper center)
self.mask2 = np.zeros((rows,col))
self.mask2[cR+rowFrac:cR+(2*rowFrac),cC-colFrac:cC+colFrac] = 5
self.mask2 = np.uint16(self.mask2)
self.mask2 = cv2.inRange(self.mask2,np.array(4,dtype = "uint16"),np.array(6,dtype = "uint16"))
# Mask to get values of specific box in z direction only interested in our object/person
min_z= np.array(500, dtype = "uint16") #bgr
max_z= np.array(2000, dtype = "uint16")
self.mask = cv2.inRange(self.depth_image, min_z, max_z)
#Combination of masks
self.mask3 = cv2.bitwise_and(self.mask,self.mask, mask= self.mask2)
image = cv2.bitwise_and(self.depth_image,self.depth_image, mask= self.mask3)
#Remove unnecesarry values
image = image.astype(float)
image[image==0] = np.nan
image = image[~np.isnan(image)]
# Get moment/centroid of object
M = cv2.moments(self.mask3)
# Calculate center of object and find error from center object
# Centroid not found
if (M['m00'] == 0):
rospy.loginfo('Not tracking ')
self.move_cmd.linear.x = 0
self.move_cmd.angular.z = 0
#Centroid found:
if ( M['m00'] > 0):
cx = int(M['m10']/M['m00'])
cy = int(M['m01']/M['m00'])
centerOfObject = (int(cx),int(cy))
dx = cx - col/2 # +ve move left, -ve move right?
dy = cy - rows/2
depth = np.median(image)
rospy.loginfo('depth is '+ str(depth))
#dz range can go from 1100
dz = depth - self.desired_thresh
#Ignore very low depth values
if depth <= self.invalid_thresh:
self.move_cmd.linear.x = 0
self.move_cmd.angular.z = K*(0)*dx
#If below low bound but higher than invalid then move backwards?
elif depth < self.desired_lowBound:
if (abs(Kx*dz) < self.max_speed):
self.move_cmd.linear.x = Kx*dz
else:
self.move_cmd.linear.x = -self.max_speed
self.move_cmd.angular.z = K*(-1)*dx
#If below upper bound but higher than low bound dont move forward but do rotate
elif depth < self.desired_upBound:
self.move_cmd.linear.x = 0*Kx
self.move_cmd.angular.z = K*(-1)*dx
#If below the invalid thresh then move forward
elif depth < self.invalid_max:
if (Kx*dz < self.max_speed):
self.move_cmd.linear.x = Kx*dz
else:
self.move_cmd.linear.x = -self.max_speed
self.move_cmd.angular.z = K*(-1)*dx
#If above threshold do nothing
elif depth >= self.invalid_max:
self.move_cmd.linear.x = 0
self.move_cmd.angular.z = K*(0)*dx
else:
self.move_cmd.linear.x = 0
self.move_cmd.angular.z = K*(0)*dx
self.cmd_vel.publish(self.move_cmd)
self.r.sleep()
except CvBridgeError, e:
print e
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.waypoints_2d = None
self.waypoints_tree = None
self.camera_image = None
self.lights = []
sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
'''
/vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and
helps you acquire an accurate ground truth data source for the traffic light
classifier by sending the current color state of all traffic lights in the
simulator. When testing on the vehicle, the color state will not be available. You'll need to
rely on the position of the light and the camera image to predict it.
'''
sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
sub6 = rospy.Subscriber('/image_color', Image, self.image_cb)
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
if not self.waypoints_2d:
self.waypoints_2d = [[
waypoint.pose.pose.position.x, waypoint.pose.pose.position.y
] for waypoint in waypoints.waypoints]
self.waypoints_tree = KDTree(self.waypoints_2d)
def traffic_cb(self, msg):
self.lights = msg.lights
def image_cb(self, msg):
"""Identifies red lights in the incoming camera image and publishes the index
of the waypoint closest to the red light's stop line to /traffic_waypoint
Args:
msg (Image): image from car-mounted camera
"""
self.has_image = True
self.camera_image = msg
light_wp, state = self.process_traffic_lights()
'''
Publish upcoming red lights at camera frequency.
Each predicted state has to occur `STATE_COUNT_THRESHOLD` number
of times till we start using it. Otherwise the previous stable state is
used.
'''
if self.state != state:
self.state_count = 0
self.state = state
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
light_wp = light_wp if state == TrafficLight.RED or state == TrafficLight.YELLOW else -1
self.last_wp = light_wp
self.upcoming_red_light_pub.publish(Int32(light_wp))
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
self.state_count += 1
def get_closest_waypoint(self, x, y):
"""Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
pose (Pose): position to match a waypoint to
Returns:
int: index of the closest waypoint in self.waypoints
"""
#TODO implement
closest_idx = self.waypoints_tree.query([x, y])[1]
return closest_idx
def get_light_state(self, light):
"""Determines the current color of the traffic light
Args:
light (TrafficLight): light to classify
Returns:
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
"""
stop_line_positions = self.config['stop_line_positions']
if(self.pose):
car_position = self.get_closest_waypoint(self.pose.pose.position.x, self.pose.pose.position.y)
nbr_wp = len(self.waypoints.waypoints)
diff = nbr_wp
for i, temp_light in enumerate(self.lights):
#Get stop line waypoint index
line = stop_line_positions[i]
temp_wp = self.get_closest_waypoint(line[0], line[1])
# Find closest stop line
dist = temp_wp - car_position
dist_next_tour = temp_wp + nbr_wp - car_position
if dist >= 0 and dist < diff:
diff = dist
light = temp_light
light_wp = temp_wp
if dist_next_tour >= 0 and dist_next_tour < diff:
diff = dist_next_tour
light = temp_light
light_wp = temp_wp
# for getting image
time = rospy.get_time()
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
state = light.state
if diff > 92:
state = 4
name = '{}_{}.png'.format(time, state)
path = '/capstone/data/imgs/'
filename = path + name
rospy.logwarn('%s dist: %s', filename, diff)
cv2.imwrite(filename, cv_image)
"""
# Just for debugging
#return light.state
if (not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#Get classification
return self.light_classifier.get_classification(cv_image)
def process_traffic_lights(self):
"""Finds closest visible traffic light, if one exists, and determines its
location and color
Returns:
int: index of waypoint closes to the upcoming stop line for a traffic light (-1 if none exists)
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
light = None
# List of positions that correspond to the line to stop in front of for a given intersection
stop_line_positions = self.config['stop_line_positions']
if (self.pose):
car_position = self.get_closest_waypoint(self.pose.pose.position.x,
self.pose.pose.position.y)
#TODO find the closest visible traffic light (if one exists)
diff = 500
for i, temp_light in enumerate(self.lights):
#Get stop line waypoint index
line = stop_line_positions[i]
temp_wp = self.get_closest_waypoint(line[0], line[1])
# Find closest stop line
dist = temp_wp - car_position
if dist >= 0 and dist < diff:
diff = dist
light = temp_light
light_wp = temp_wp
if light:
state = self.get_light_state(light)
return light_wp, state
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.tl_wps = []
sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
'''
/vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and
helps you acquire an accurate ground truth data source for the traffic light
classifier by sending the current color state of all traffic lights in the
simulator. When testing on the vehicle, the color state will not be available. You'll need to
rely on the position of the light and the camera image to predict it.
'''
sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_cb)
sub6 = rospy.Subscriber('/image_color', Image, self.image_cb)
config_string = rospy.get_param("/traffic_light_config")
rospy.logwarn("The parameter string is : %s", config_string )
self.config = yaml.load(config_string)
self.upcoming_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1)
# debug: publisher of cross-correlation results
self.ccresult_pub = rospy.Publisher('/traffic_ccresult', Image, queue_size=1)
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.count = 0
self.camera_car_position = []
# x-margin is the margin in pixels that corrspondes to a
# 5 meter distance from the traffic light along the y-axis (x in image).
# we use this margin to search for the red pixels instead of a fixed one
self.x_margin = 50 # default is 50 (Yuda)
# dimensions of the red-light template in pixels
self.template_x = 30 # (pixels in x) just a wild-guess for initial value.
self.template_y = 30 # (pixels in y)
# the actual dimension of a single light as a swuare
self.TrafficLight_dim = 1.6# (suppose to be in meters but scaling is a bit hand-tuned)
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
def traffic_cb(self, msg):
self.lights = msg.lights
def image_cb(self, msg):
"""Identifies red lights in the incoming camera image and publishes the index
of the waypoint closest to the red light's stop line to /traffic_waypoint
Args:
msg (Image): image from car-mounted camera
"""
self.has_image = True
self.camera_image = msg
# self.camera_car_position.append(self.pose.pose.position)
# if len(self.camera_car_position) > DELAY:
# self.camera_car_position.pop(0)
light_wp, state = self.process_traffic_lights()
'''
Publish upcoming red lights at camera frequency.
Each predicted state has to occur `STATE_COUNT_THRESHOLD` number
of times till we start using it. Otherwise the previous stable state is
used.
'''
if self.state != state:
self.state_count = 0
self.state = state
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
if state == TrafficLight.UNKNOWN:
self.upcoming_light_pub.publish(Int32(-1))
return
self.last_wp = light_wp
self.upcoming_light_pub.publish(Int32( light_wp | (self.state << 16) ))
else:
self.upcoming_light_pub.publish(Int32(self.last_wp | (self.last_state << 16) ))
self.state_count += 1
def get_closest_waypoint(self, pose):
"""Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
pose (Pose): position to match a waypoint to
Returns:
int: index of the closest waypoint in self.waypoints
"""
#TODO implement
closest_index = -1
closest_dis = -1
if self.waypoints is not None:
wps = self.waypoints.waypoints
for i in range(len(wps)):
dis = (wps[i].pose.pose.position.x - pose.x) ** 2 + \
(wps[i].pose.pose.position.y - pose.y) ** 2
if (closest_dis == -1) or (closest_dis > dis):
closest_dis = dis
closest_index = i
return closest_index
# George: Here's my version of project_to_image_plane
# I am avoiding the TransformListener object as I am not sure about how
# to configure it without having doubts. The transform is easy to
# work-out directly from the pose vector and gives me control over the
# coordinate frame.
def project_to_image_plane(self, point_in_world):
"""Project point from 3D world coordinates to 2D camera image location
Args:
point_in_world (Point): 3D location of a point in the world
Returns:
x (int): x coordinate of target point in image
y (int): y coordinate of target point in image
"""
# Retreving camera intronsics
fx = self.config['camera_info']['focal_length_x']
fy = self.config['camera_info']['focal_length_y']
# image size
image_width = self.config['camera_info']['image_width']
image_height = self.config['camera_info']['image_height']
# caching the world coordinates of the point
Px = point_in_world.x
Py = point_in_world.y
Pz = point_in_world.z
# Using the pose to obatin the camera frame as rotation matrix R and
# a world position p (NOTEL ASSUMING THAT THE CAMERA COINCIDES WITH
# THE CAR'S BARYCENTER
# Cx = self.camera_car_position[0].x
# Cy = self.camera_car_position[0].y
# Cz = self.camera_car_position[0].z
Cx = self.pose.pose.position.x
Cy = self.pose.pose.position.y
Cz = self.pose.pose.position.z
# print(Px, Py, Pz)
# print(Cx, Cy, Cz)
# print('=========')
# get orientation (just the scalar part of the the quaternion)
s = self.pose.pose.orientation.w # quaternion scalar
# now obtaining orientation of the car (assuming rotation about z: [0;0;1])
theta = 2 * np.arccos(s)
# Constraining the angle in [-pi, pi)
if theta > np.pi:
theta = -(2 * np.pi - theta)
# transforming the world point to the camera frame as:
#
# Pc = R' * (Pw - C)
# where R' = [ cos(theta) sin(theta) 0;
# -sin(theta) cos(theta) 0;
# 0 0 1]
#
# Thus,
p_camera = [ np.cos(theta) * (Px - Cx) + np.sin(theta) * (Py - Cy) , \
-np.sin(theta) * (Px - Cx) + np.cos(theta) * (Py - Cy) , \
Pz - Cz]
# print(p_camera)
# NOTE: From the simulator, it appears from the change in the angle
# that the positive direction of rotation is counter-clockwise. This
# means that there are two possible frame arrangements:
#
# a) A RIGHT-HAND frame: In this frame, z - points upwards (oposite to the
# image y axis)and y points to the left (oposite to the image x-axis)
#
# b) A LEFT_HAND frame: In this frame, z-points downwards (same as the
# image y-axis) and y points left (oposite to the image x-axis).
#
# thus, there are two ways of obtaining the image projection:
# =============== Udacity Forums ====================================
# see https://discussions.udacity.com/t/focal-length-wrong/358568
# if fx < 10:
# fx = 2574
# fy = 2744
# p_camera[2] -= 1.0 # to account for the elevation of the camera
# c_x = image_width/2 - 30
# c_y = image_height + 50
# x = int(-fx * p_camera[1] / p_camera[0] + c_x)
# y = int(-fy * p_camera[2] / p_camera[0] + c_y)
# ===================================================================
# =============== Using actual (given) intrinsics ================
if fx < 3 or fy < 3:
# This means that intrinsics have been normalized
# so that they can be applied at any resolution
fx = fx * image_width
fy = fy * image_height
# MAYBE, assume that camera is 1 m above the ground (not used)
#p_camera[2] -= 1
# Since we dont know the intersection of the optical axis with the image,
# we assume it lies in the middle of it. It should work roughly....
c_x = image_width / 2
c_y = image_height / 2
x = int(-fx * p_camera[1] / p_camera[0] + c_x)
y = int(+fy * p_camera[2] / p_camera[0] + c_y)
# ===============================================================
# Set the ADAPTIVE search margin for the traffic light.
self.x_margin = int(np.abs(6 * fx / p_camera[0])) # note the 5 metert factor
#rospy.logwarn("X-Margin : %i", self.x_margin)
if (self.x_margin < 20):
self.x_margin = 20 # use default 20 pixels if too small margin
if (self.x_margin > 200): # use maximum margin 200 pixels (just to avoid large roi images when out of range)
self.x_margin = 200
# Now working out the size (dimension) of the traffic light
# (NOTE: ONLY a single light - i.e. on of the three) in pixels
self.template_x = int( fx * self.TrafficLight_dim / p_camera[0] )
if (self.template_x > 45 ):
self.template_x = 45
self.template_y = int( fy * self.TrafficLight_dim / p_camera[0] )
if (self.template_y > 45 ):
self.template_y = 45
return (x,y)
def in_image(self, x, y):
if x is None or y is None:
return False
if x < 0 or x >= self.config['camera_info']['image_width']:
return False
if y < 0 or y >= self.config['camera_info']['image_height']:
return False
return True
def get_light_state(self, light):
"""Determines the current color of the traffic light
Args:
light (TrafficLight): light to classify
Returns:
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
if(not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
x, y = self.project_to_image_plane(light.pose.pose.position)
#TODO use light location to zoom in on traffic light in image
image_width = self.config['camera_info']['image_width']
image_height = self.config['camera_info']['image_height']
# Actually this range is enough.
# But we just use a bigger rectangle to make sure the light is in this region
# ret = cv2.rectangle(cv_image, (x-20,y-50), (x+20,y+50), (0, 0, 255))
#left = x - 50
left = x - self.x_margin
#right = x + 50
right = x + self.x_margin
top = 20
bottom = y + 100
state = TrafficLight.UNKNOWN
#if self.in_image(left, top) and self.in_image(right, bottom):
roi = cv_image[top:bottom, left:right]
# skip if roi not set (it may happen, despite the in_image tests)
# NOTE: This
if (roi is None):
rospy.logwarn("ROI image is None!")
return state
# skip if roi is singular (i.e. single row or column)
if (roi.shape[0] < 2 or roi.shape[1] < 2):
return state
# bridge will be useful publishing the classification results
bridge = CvBridge()
if (self.template_x > 5) and (self.template_y):
(ccres, state) = self.light_classifier.matchRedTemplate(cv_image, self.template_x, self.template_y)
# publish the results
if not (ccres is None):
self.ccresult_pub.publish(bridge.cv2_to_imgmsg(ccres, "bgr8"))
self.count += 1
# perform light state classification
#state = self.light_classifier.get_classification(roi)
#rospy.logwarn("TL state classified: %d, state count %d", state, self.state_count)
# debug only
# if self.count > STATE_COUNT_THRESHOLD and self.count < 10: # save some imgs, not all
# cv2.imwrite('/home/student/Tests/imgs/' + ("%.3d-%d" % (self.count, state)) + '.jpg', roi)
return state
def track_index_diff(self,index1, index2):
if (self.waypoints.waypoints is None):
return -1
N = len(self.waypoints.waypoints)
if index2 >= index1 :
return index2 - index1
else:
return N - index1 - 1 + index2
def process_traffic_lights(self):
"""Finds closest visible traffic light, if one exists, and determines its
location and color
Returns:
int: index of waypoint closes to the upcoming stop line for a traffic light (-1 if none exists)
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
if (self.waypoints is None):
return (-1, TrafficLight.UNKNOWN)
# List of positions that correspond to the line to stop in front of for a given intersection
stop_line_positions = self.config['stop_line_positions']
# now, for all stop_lines find nearest points (shoudl be done ONCE
if (len(self.tl_wps)==0):
for i, stop_line in enumerate(stop_line_positions):
tl_wp = self.get_closest_waypoint(Point(stop_line))
self.tl_wps.append( (tl_wp+5) % len(self.waypoints.waypoints) ) # +1 to give extra margin towards the traffic light
light = None
if(self.pose):
car_wp = self.get_closest_waypoint(self.pose.pose.position)
# Now find the smallest distance to a traffic light wp from the car wp:
minDist = self.track_index_diff(car_wp, self.tl_wps[0])
light_index = 0
for i in range(1, len(self.tl_wps)):
dist = self.track_index_diff(car_wp, self.tl_wps[i])
if dist > 150 / 0.63: #about 150 meters ON-TRACK
continue
if (dist < minDist):
minDist = dist
light_index = i
light = self.lights[light_index]
#print("Nearest Traffic light index : ", light_index)
if light:
# print(light_wp, self.count)
state = self.get_light_state(light)
return self.tl_wps[light_index], state
return -1, TrafficLight.UNKNOWN
def process_traffic_lights_old(self):
"""Finds closest visible traffic light, if one exists, and determines its
location and color
Returns:
int: index of waypoint closes to the upcoming stop line for a traffic light (-1 if none exists)
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
light = None
# List of positions that correspond to the line to stop in front of for a given intersection
stop_line_positions = self.config['stop_line_positions']
if(self.pose):
car_position = self.get_closest_waypoint(self.pose.pose.position)
#TODO find the closest visible traffic light (if one exists)
light_wp = -1
for i, stop_line in enumerate(stop_line_positions):
# computing the distance from the car to the traffic-light stop line
dis = (stop_line[0] - self.pose.pose.position.x)**2 + \
(stop_line[1] - self.pose.pose.position.y)**2
# if the distance beyond a threshold (200^2 = 40000) skip
if dis > MAX_DISTANCE_SQR:
continue
# Now, if the traffic light is potentially visible, find the
# closest waypoint to it
stop_line_wp = self.get_closest_waypoint(Point(stop_line))
if stop_line_wp >= car_position:
if (light_wp == -1) or (light_wp > stop_line_wp):
print(stop_line_wp, car_position, light_wp)
light_wp = stop_line_wp
light = self.lights[i]
#rospy.logwarn("Found closest traffic light (waypoint) : %i" , light_wp)
if light:
# print(light_wp, self.count)
state = self.get_light_state(light)
return light_wp, state
# self.waypoints = None
return -1, TrafficLight.UNKNOWN
class BlobDetector:
def __init__(self):
self.bridge = CvBridge()
self.map_frame_id = rospy.get_param('~map_frame_id', 'map')
self.frame_id = rospy.get_param('~frame_id', 'base_link')
self.object_frame_id = rospy.get_param('~object_frame_id', 'object')
self.color_hue = rospy.get_param('~color_hue',
10) # 160=purple, 100=blue, 10=Orange
self.color_range = rospy.get_param('~color_range', 15)
self.color_saturation = rospy.get_param('~color_saturation', 50)
self.color_value = rospy.get_param('~color_value', 50)
self.border = rospy.get_param('~border', 10)
self.config_srv = Server(BlobDetectorConfig, self.config_callback)
params = cv2.SimpleBlobDetector_Params()
# see https://www.geeksforgeeks.org/find-circles-and-ellipses-in-an-image-using-opencv-python/
# https://docs.opencv.org/3.4/d0/d7a/classcv_1_1SimpleBlobDetector.html
params.thresholdStep = 10
params.minThreshold = 50
params.maxThreshold = 220
params.minRepeatability = 2
params.minDistBetweenBlobs = 10
# Set Color filtering parameters
params.filterByColor = False
params.blobColor = 255
# Set Area filtering parameters
params.filterByArea = True
params.minArea = 10
params.maxArea = 5000000000
# Set Circularity filtering parameters
params.filterByCircularity = True
params.minCircularity = 0.3
# Set Convexity filtering parameters
params.filterByConvexity = False
params.minConvexity = 0.95
# Set inertia filtering parameters
params.filterByInertia = False
params.minInertiaRatio = 0.1
self.detector = cv2.SimpleBlobDetector_create(params)
self.br = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
self.image_pub = rospy.Publisher('image_detections',
Image,
queue_size=1)
self.object_pub = rospy.Publisher('object_detected',
String,
queue_size=1)
self.image_sub = message_filters.Subscriber('image', Image)
self.depth_sub = message_filters.Subscriber('depth', Image)
self.info_sub = message_filters.Subscriber('camera_info', CameraInfo)
self.ts = message_filters.TimeSynchronizer(
[self.image_sub, self.depth_sub, self.info_sub], 10)
self.ts.registerCallback(self.image_callback)
def __del__(self):
self.file.close()
def config_callback(self, config, level):
rospy.loginfo(
"""Reconfigure Request: {color_hue}, {color_saturation}, {color_value}, {color_range}, {border}"""
.format(**config))
self.color_hue = config.color_hue
self.color_range = config.color_range
self.color_saturation = config.color_saturation
self.color_value = config.color_value
self.border = config.border
return config
def image_callback(self, image, depth, info):
try:
cv_image = self.bridge.imgmsg_to_cv2(image, "bgr8")
except CvBridgeError as e:
print(e)
try:
cv_depth = self.bridge.imgmsg_to_cv2(depth, "32FC1")
except CvBridgeError as e:
print(e)
hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(
hsv,
np.array([
self.color_hue - self.color_range, self.color_saturation,
self.color_value
]), np.array([self.color_hue + self.color_range, 255, 255]))
keypoints = self.detector.detect(mask)
closestObject = [
0, 0, 0
] # Object pose (x,y,z) in camera frame (x->right, y->down, z->forward)
if len(keypoints) > 0:
cv_image = cv2.drawKeypoints(
cv_image, keypoints, np.array([]), (0, 0, 255),
cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
for i in range(0, len(keypoints)):
if info.K[0] > 0 and keypoints[i].pt[
0] >= self.border and keypoints[i].pt[
0] < cv_image.shape[1] - self.border:
pts_uv = np.array(
[[[keypoints[i].pt[0], keypoints[i].pt[1]]]],
dtype=np.float32)
info_K = np.array(info.K).reshape([3, 3])
info_D = np.array(info.D)
info_P = np.array(info.P).reshape([3, 4])
pts_uv = cv2.undistortPoints(pts_uv, info_K, info_D,
info_P)
angle = np.arcsin(
-pts_uv[0][0][0]
) # negative to get angle from forward x axis
x = pts_uv[0][0][0]
y = pts_uv[0][0][1]
#rospy.loginfo("(%d/%d) %f %f -> %f %f angle=%f deg", i+1, len(keypoints), keypoints[i].pt[0], keypoints[i].pt[1], x, y, angle*180/np.pi)
# Get depth.
u = int(x * info.P[0] + info.P[2])
v = int(y * info.P[5] + info.P[6])
depth = -1
if u >= 0 and u < cv_depth.shape[1]:
for j in range(0, cv_depth.shape[0]):
if cv_depth[j, u] > 0:
depth = cv_depth[j, u]
break
# is the depth contained in the blob?
if abs(j - v) < keypoints[i].size / 2:
depth = cv_depth[j, u]
break
if depth > 0 and (closestObject[2] == 0
or depth < closestObject[2]):
closestObject[0] = x * depth
closestObject[1] = y * depth
closestObject[2] = depth
# We process only the closest object detected
if closestObject[2] > 0:
# assuming the object is circular, use center of the object as position
transObj = (closestObject[0], closestObject[1], closestObject[2])
rotObj = tf.transformations.quaternion_from_euler(
0, np.pi / 2, -np.pi / 2)
self.br.sendTransform(transObj, rotObj, image.header.stamp,
self.object_frame_id, image.header.frame_id)
msg = String()
msg.data = self.object_frame_id
self.object_pub.publish(
msg) # signal that an object has been detected
# Compute object pose in map frame
try:
self.listener.waitForTransform(self.map_frame_id,
image.header.frame_id,
image.header.stamp,
rospy.Duration(0.5))
(transMap, rotMap) = self.listener.lookupTransform(
self.map_frame_id, image.header.frame_id,
image.header.stamp)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
print(e)
return
(transMap, rotMap) = multiply_transforms((transMap, rotMap),
(transObj, rotObj))
# Compute object pose in base frame
try:
self.listener.waitForTransform(self.frame_id,
image.header.frame_id,
image.header.stamp,
rospy.Duration(0.5))
(transBase, rotBase) = self.listener.lookupTransform(
self.frame_id, image.header.frame_id, image.header.stamp)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
print(e)
return
(transBase, rotBase) = multiply_transforms((transBase, rotBase),
(transObj, rotObj))
distance = np.linalg.norm(transBase[0:2])
angle = np.arcsin(transBase[1] / transBase[0])
rospy.loginfo(
"Object detected at [%f,%f] in %s frame! Distance and direction from robot: %fm %fdeg.",
transMap[0], transMap[1], self.map_frame_id, distance,
angle * 180.0 / np.pi)
# debugging topic
if self.image_pub.get_num_connections() > 0:
cv_image = cv2.bitwise_and(cv_image, cv_image, mask=mask)
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.safe_visualizations = False # saves camera images with detected bounding boxes
self.use_classifier = True # use the traffic light detection
self.waypoint_range = 200
self.current_tl_wp_idx = 0
self.pose = None
self.waypoints = None
self.waypoints_2d = None
self.waypoints_tree = None
self.camera_image = None
self.lights = []
self.light_classifier = TLClassifier(self.safe_visualizations)
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
'''
/vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and
helps you acquire an accurate ground truth data source for the traffic light
classifier by sending the current color state of all traffic lights in the
simulator. When testing on the vehicle, the color state will not be available. You'll need to
rely on the position of the light and the camera image to predict it.
'''
rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
rospy.Subscriber('/image_color', Image, self.image_cb)
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
self.stop_line_positions = self.config['stop_line_positions']
# rospy.logwarn(self.config)
self.is_site = self.config['is_site']
output_dir = os.path.join('./data/dataset/',
'site' if self.is_site else 'simulator')
self.image_saver = ImageSaver(None, output_dir)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.bridge = CvBridge()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.no_of_wp = None
self.idx_of_closest_wp_to_car = None
self.line_wp_idxs_list = []
self.has_image = False
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
self.no_of_wp = len(self.waypoints.waypoints)
if not self.waypoints_2d:
self.waypoints_2d = [[
waypoint.pose.pose.position.x, waypoint.pose.pose.position.y
] for waypoint in waypoints.waypoints]
self.waypoints_tree = KDTree(self.waypoints_2d)
def traffic_cb(self, msg):
self.lights = msg.lights
# rospy.logwarn(msg.lights)
def image_cb(self, msg):
"""Identifies red lights in the incoming camera image and publishes the index
of the waypoint closest to the red light's stop line to /traffic_waypoint
Args:
msg (Image): image from car-mounted camera
"""
if self.is_site:
self.has_image = True
self.camera_image = msg
tl_in_range = self.traffic_light_in_range()
light_wp, state = self.process_traffic_lights()
'''
Publish upcoming red lights at camera frequency.
Each predicted state has to occur `STATE_COUNT_THRESHOLD` number
of times till we start using it. Otherwise the previous stable state is
used.
'''
if self.state != state:
self.state_count = 0
self.state = state
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
light_wp = light_wp if state == TrafficLight.RED else -1
self.last_wp = light_wp
self.upcoming_red_light_pub.publish(Int32(light_wp))
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
self.state_count += 1
else:
if self.waypoints_tree and self.light_classifier:
tl_in_range = self.traffic_light_in_range()
rospy.logwarn("Car wp: {}, TL in range: {}".format(
self.idx_of_closest_wp_to_car, tl_in_range))
if tl_in_range:
rospy.logwarn(
"TL wp: {}, TL idx: {} in range {} wp".format(
self.line_wp_idxs_list[
self.current_tl_wp_idx %
len(self.line_wp_idxs_list)],
self.current_tl_wp_idx, self.waypoint_range))
self.has_image = True
self.camera_image = msg
light_wp, state = self.process_traffic_lights()
'''
Publish upcoming red lights at camera frequency.
Each predicted state has to occur `STATE_COUNT_THRESHOLD` number
of times till we start using it. Otherwise the previous stable state is
used.
'''
if self.state != state:
self.state_count = 0
self.state = state
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
light_wp = light_wp if state == TrafficLight.RED or state == TrafficLight.YELLOW else -1
self.last_wp = light_wp
self.upcoming_red_light_pub.publish(Int32(light_wp))
else:
self.upcoming_red_light_pub.publish(Int32(
self.last_wp))
self.state_count += 1
def traffic_light_in_range(self):
"""Identifies if the closest traffic light to the vehicles position
is within a given number of waypoints ahead of the vehicle.
Returns:
boolean: if a traffic light satisfies these parameters or not
"""
offest = 25 # approx offset (in waypoints) from the traffic light and the stop line.
if self.pose:
self.idx_of_closest_wp_to_car = self.get_closest_waypoint(
self.pose.pose.position.x, self.pose.pose.position.y)
if len(self.line_wp_idxs_list) < 1:
for i, light in enumerate(self.lights):
line = self.stop_line_positions[i]
self.line_wp_idxs_list.append(
self.get_closest_waypoint(line[0], line[1]))
if self.idx_of_closest_wp_to_car \
> self.line_wp_idxs_list[self.current_tl_wp_idx % len(self.line_wp_idxs_list)]:
self.current_tl_wp_idx += 1
if self.idx_of_closest_wp_to_car + self.waypoint_range \
> self.line_wp_idxs_list[self.current_tl_wp_idx % len(self.line_wp_idxs_list)] + offest:
return True
else:
return False
def get_closest_waypoint(self, x, y):
"""Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
x (float): position to match a waypoint to
y (float): position to match a waypoint to
Returns:
int: index of the closest waypoint in self.waypoints
"""
closest_idx = self.waypoints_tree.query([x, y], 1)[1]
return closest_idx
def get_light_state(self, light):
"""Determines the current color of the traffic light
Args:
light (TrafficLight): light to classify
Returns:
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
if not self.has_image:
return False
if not self.use_classifier:
rospy.loginfo('Light state: %s', light.state)
return light.state
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, 'rgb8')
classified_state = self.light_classifier.get_classification(cv_image)
if not self.is_site:
rospy.loginfo("Sim ground truth state: {}".format(
self.state_to_string(light.state)))
rospy.loginfo("Classified state: {}".format(
self.state_to_string(classified_state)))
return classified_state
@staticmethod
def state_to_string(state):
if state == TrafficLight.RED:
return "Red"
elif state == TrafficLight.YELLOW:
return "Yellow"
elif state == TrafficLight.GREEN:
return "Green"
return "Unknown"
def process_traffic_lights(self):
"""Finds closest visible traffic light, if one exists, and determines its
location and color
Returns:
int: index of waypoint closes to the upcoming stop line for a traffic light (-1 if none exists)
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
closest_light = None
line_wp_idx = None
if self.pose:
diff = self.no_of_wp
for i, light in enumerate(self.lights):
idx_of_closest_wp_to_line = self.line_wp_idxs_list[i]
d = idx_of_closest_wp_to_line - self.idx_of_closest_wp_to_car
if diff > d >= 0:
diff = d
closest_light = light
line_wp_idx = idx_of_closest_wp_to_line
if closest_light:
return line_wp_idx, self.get_light_state(closest_light)
return -1, TrafficLight.UNKNOWN
