Skip to content
Snippets Groups Projects
Commit c2e044f1 authored by Jerry Kuo's avatar Jerry Kuo
Browse files

Depth Detection Implementation

parent 97ba2a7b
No related branches found
No related tags found
No related merge requests found
/.idea .idea
.bag *.bag
.txt *CMakeLists.txt
.catkin_workspace .catkin_workspace
#!/usr/bin/env python
import sys
import cv2
import rospy
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
text_position = (10, 50)
font = cv2.FONT_HERSHEY_SIMPLEX
font_scale = 0.75
font_color = (255, 255, 255)
line_type = 2
class obj_dist:
def __init__(self):
# Initializing ROS Topics
self.bridge = CvBridge()
self.measure_pub = rospy.Publisher('/obj_to_dist/measuring', Image, queue_size=10)
self.d_sub = rospy.Subscriber('/camera/depth/image_rect_raw', Image, self.fixed_distance)
def fixed_distance(self, data):
try:
cv_depth = self.bridge.imgmsg_to_cv2(data, 'passthrough')
except CvBridgeError as e:
print(e)
return
cv2.rectangle(cv_depth, (250, 190), (350, 290), (255, 0, 0), 5)
roi_depth = cv_depth[250:350, 190:290]
# roi_depth = cv_depth[250:255, 190:195]
# Ten thousand pixels, divide by 1000 for meters
# avg_distance = np.sum(roi_depth[:, :])/10000/1000
filtered_depth = roi_depth[roi_depth <= 2900]
_size = len(filtered_depth)
avg_distance = filtered_depth.sum() / _size / 1000
cv2.putText(cv_depth, 'Object Distance: {} meters'.format(avg_distance),
text_position,
font,
font_scale,
font_color,
line_type)
try:
ret_img = self.bridge.cv2_to_imgmsg(cv_depth, 'passthrough')
except CvBridgeError as e:
print(e)
return
self.measure_pub.publish(ret_img)
def main(args):
rospy.init_node('obj_dist', anonymous=True)
_ = obj_dist()
rospy.spin()
cv2.destroyAllWindows()
if __name__ == '__main__':
rospy.set_param("use_sim_time", 'true')
main(sys.argv)
...@@ -3,35 +3,72 @@ ...@@ -3,35 +3,72 @@
import numpy as np import numpy as np
import sys import sys
import cv2 import cv2
import rospy import rospy
import pyrealsense2 as rs2
import message_filters import message_filters
sys.path.append('/home/jerry/Documents/workspaces/ROS_human_detection/src/human_detection')
from sensor_msgs import point_cloud2 as pcl
from sensor_msgs.msg import Image, PointCloud2 from sensor_msgs.msg import Image, PointCloud2
from cv_bridge import CvBridge, CvBridgeError from cv_bridge import CvBridge, CvBridgeError
from human_detection.msg import bounding_box, box_list
text_position = (10, 50)
font = cv2.FONT_HERSHEY_SIMPLEX
font_scale = 0.75
font_color = (255, 255, 255)
line_type = 2
counter = 0
class obj_dist: class obj_dist:
def __init__(self): def __init__(self):
# Initializing ROS Topics # Initializing ROS Topics
self.bridge = CvBridge() self.bridge = CvBridge()
dist_pub = rospy.Publisher('/obj_to_dist', Image, queue_size=10) self.dist_pub = rospy.Publisher('/obj_to_dist/human_distance', Image, queue_size=10)
self.human_sub = message_filters.Subscriber('/camera/color/image_raw', Image)
self.depth_sub = message_filters.Subscriber('/camera/depth_registered/points', PointCloud2) # self.bbx_sub = rospy.Subscriber('/human_detected_image/bounding_box', box_list, self.yea)
# self.human_image_sub = rospy.Subscriber('/human_detected_image/image', Image, self.yea1)
# self.depth_image_sub = rospy.Subscriber('/camera/depth/image_rect_raw', Image, self.yea2)
self.bbx_sub = message_filters.Subscriber('/human_detected_image/bounding_box', box_list)
self.human_image_sub = message_filters.Subscriber('/human_detected_image/image', Image)
self.depth_image_sub = message_filters.Subscriber('/camera/depth/image_rect_raw', Image)
ts = message_filters.TimeSynchronizer([self.human_sub, self.depth_sub], 10) ts = message_filters.ApproximateTimeSynchronizer([self.bbx_sub, self.human_image_sub, self.depth_image_sub],
queue_size=10,
slop=1)
ts.registerCallback(self.callback) ts.registerCallback(self.callback)
def callback(self, Image, Depth): def callback(self, bbx, image, depth):
print('wtf') print('Called!')
global counter
if bbx.length:
cv_depth = self.bridge.imgmsg_to_cv2(depth, 'passthrough')
cv_image = self.bridge.imgmsg_to_cv2(image, 'bgr8')
box = bbx.people_list[0]
roi_depth = cv_depth[box.xmin:box.xmax, box.ymin:box.ymax]
x = box.xmax - box.xmin
y = box.ymax - box.ymin
avg_distance = roi_depth[roi_depth <= 2500].sum() / (x*y) / 1000
cv2.putText(cv_image, '{} meters'.format(avg_distance),
(box.xmin, box.ymax-100),
font,
font_scale,
font_color,
line_type)
self.dist_pub.publish(self.bridge.cv2_to_imgmsg(cv_image))
counter += 1
def main(args): def main(args):
_ = obj_dist()
rospy.init_node('obj_dist', anonymous=True) rospy.init_node('obj_dist', anonymous=True)
_ = obj_dist()
rospy.spin() rospy.spin()
cv2.destroyAllWindows() cv2.destroyAllWindows()
if __name__ == '__main__': if __name__ == '__main__':
# rospy.set_param("use_sim_time", 'true')
main(sys.argv) main(sys.argv)
from datetime import datetime
import pyrealsense2 as rs
import numpy as np
import open3d as o3d
import time
# Create a pipeline
pipeline = rs.pipeline()
# Create a config and configure the pipeline to stream
# different resolutions of color and depth streams
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30)
# Start streaming
profile = pipeline.start(config)
# Streaming loop
try:
vis = o3d.visualization.Visualizer()
vis.create_window("Tests")
pcd = o3d.geometry.PointCloud()
while True:
dt0 = datetime.now()
vis.add_geometry(pcd)
pcd.clear()
# Get frameset of color and depth
frames = pipeline.wait_for_frames()
color = frames.get_color_frame()
depth = frames.get_depth_frame()
if not color or not depth:
continue
pc = rs.pointcloud()
pc.map_to(color)
points = pc.calculate(depth)
vtx = np.asanyarray(points.get_vertices())
print(vtx)
print(o3d.utility.Vector3dVector(vtx))
# pcd.points = o3d.utility.Vector3dVector(vtx)
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
process_time = datetime.now() - dt0
print("FPS = {0}".format(1/process_time.total_seconds()))
time.sleep(1)
except KeyboardInterrupt:
print('Exiting')
finally:
pipeline.stop()
\ No newline at end of file
#!/usr/bin/env python
import numpy as np
import open3d as o3d
import os
import time
abs_path = '/home/jerry/Documents/Research/Data/plc/file1'
def main():
vis = o3d.visualization.Visualizer()
vis.create_window("3D Scene")
# o3d.visualization.draw_geometries([pcd])
try:
while True:
for filename in os.listdir(abs_path):
print(filename)
pcd = o3d.io.read_point_cloud(abs_path+'/'+filename)
time.sleep(0.1)
vis.add_geometry(pcd)
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
# o3d.visualization.Visualizer.draw_geometrie(pcd)
except KeyboardInterrupt:
print("Terminating")
if __name__ == "__main__":
main()
import cv2
print(cv2.__version__)
video = cv2.VideoCapture(1)
while True:
ret, frame = video.read()
cv2.imshow('Object detector', frame)
# Press 'q' to quit
if cv2.waitKey(1) == ord('q'):
break
std_msgs/Header header
int8 length
bounding_box[] people_list
\ No newline at end of file
No preview for this file type
...@@ -31,9 +31,9 @@ class image_converter: ...@@ -31,9 +31,9 @@ class image_converter:
cv2.waitKey(3) cv2.waitKey(3)
try: try:
ret_val = self.bridge.cv2_to_imgmsg(cv_image, "bgr8") ret_img = self.bridge.cv2_to_imgmsg(cv_image, "bgr8")
ret_val.header = data.header ret_img.header = data.header
self.image_pub.publish(ret_val) self.image_pub.publish(ret_img)
except CvBridgeError as e: except CvBridgeError as e:
print(e) print(e)
......
...@@ -4,15 +4,15 @@ ...@@ -4,15 +4,15 @@
import time import time
import sys import sys
import os import os
import rospy
import tensorflow as tf import tensorflow as tf
sys.path.append('/home/jerry/Documents/workspaces/ROS_human_detection/src/human_detection') sys.path.append('/home/jerry/Documents/workspaces/ROS_human_detection/src/human_detection')
# import numpy as np
import rospy
# For performance analysis timing, import time. # For performance analysis timing, import time.
from analysis_tools.data_grapher import * from analysis_tools.data_grapher import *
from analysis_tools.define import * from analysis_tools.define import *
from human_detection.msg import bounding_box from human_detection.msg import bounding_box, box_list
from sensor_msgs.msg import Image from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError from cv_bridge import CvBridge, CvBridgeError
...@@ -21,7 +21,6 @@ from object_detection.utils import label_map_util ...@@ -21,7 +21,6 @@ from object_detection.utils import label_map_util
from object_detection.utils import visualization_utils as vis_util from object_detection.utils import visualization_utils as vis_util
# Defining Paths # Defining Paths
print('Current Tensorflow Version: ' + str(tf.__version__))
CWD_PATH = '/home/jerry/Documents/workspaces/ROS_human_detection/src/human_detection/src' CWD_PATH = '/home/jerry/Documents/workspaces/ROS_human_detection/src/human_detection/src'
LABEL_MAPS = ['human_label_map.pbtxt', 'mscoco_label_map.pbtxt'] LABEL_MAPS = ['human_label_map.pbtxt', 'mscoco_label_map.pbtxt']
MODEL_NAME = 'ssd_mobilenet_v1_coco_2017_11_17' MODEL_NAME = 'ssd_mobilenet_v1_coco_2017_11_17'
...@@ -68,7 +67,7 @@ class human_detector: ...@@ -68,7 +67,7 @@ class human_detector:
self.bridge = CvBridge() self.bridge = CvBridge()
self.image_pub = rospy.Publisher( self.image_pub = rospy.Publisher(
"/human_detected_image/image", Image, queue_size=10) "/human_detected_image/image", Image, queue_size=10)
self.bbx_pub = rospy.Publisher('/human_detected_image/bounding_box', bounding_box, queue_size=10) self.bbx_pub = rospy.Publisher('/human_detected_image/bounding_box', box_list, queue_size=10)
self.image_sub = rospy.Subscriber( self.image_sub = rospy.Subscriber(
"/camera/color/image_raw", Image, self.callback) "/camera/color/image_raw", Image, self.callback)
...@@ -109,12 +108,12 @@ class human_detector: ...@@ -109,12 +108,12 @@ class human_detector:
min_score_thresh=0.60) min_score_thresh=0.60)
# Calculate frame time # Calculate frame time
# t1 = time.time() t1 = time.time()
# run_time = t1 - t0 run_time = t1 - t0
# if run_time < 1: if run_time < 1:
# run_time_list.append(run_time) run_time_list.append(run_time)
# if len(run_time_list) > 10: if len(run_time_list) > 10:
# del run_time_list[0] del run_time_list[0]
if len(run_time_list) > 0: if len(run_time_list) > 0:
avg_run_time = round(sum(run_time_list) / len(run_time_list) * 1000, 1) avg_run_time = round(sum(run_time_list) / len(run_time_list) * 1000, 1)
...@@ -124,21 +123,32 @@ class human_detector: ...@@ -124,21 +123,32 @@ class human_detector:
font_scale, font_scale,
font_color, font_color,
line_type) line_type)
counter = 0 counter = 0
list_length = 0
bbx_list = box_list()
bbx_list.header.stamp = rospy.Time.now()
for score in scores[0]: for score in scores[0]:
if score > 0.6: if score > 0.6:
list_length += 1
coordinates = bounding_box() coordinates = bounding_box()
coordinates.ymin = int(boxes[0, counter, 0] * screen_height) coordinates.ymin = int(boxes[0, counter, 0] * screen_height)
coordinates.xmin = int(boxes[0, counter, 1] * screen_width) coordinates.xmin = int(boxes[0, counter, 1] * screen_width)
coordinates.ymax = int(boxes[0, counter, 2] * screen_height) coordinates.ymax = int(boxes[0, counter, 2] * screen_height)
coordinates.xmax = int(boxes[0, counter, 3] * screen_width) coordinates.xmax = int(boxes[0, counter, 3] * screen_width)
self.bbx_pub.publish(coordinates) bbx_list.people_list.append(coordinates)
counter += 1 counter += 1
print(list_length)
bbx_list.length = list_length
self.bbx_pub.publish(bbx_list)
try: try:
cvt_msg = self.bridge.cv2_to_imgmsg(cv_image, "bgr8") cvt_msg = self.bridge.cv2_to_imgmsg(cv_image, "bgr8")
cvt_msg.header = data.header cvt_msg.header.stamp = rospy.Time.now()
self.image_pub.publish(cvt_msg) self.image_pub.publish(cvt_msg)
# print(cvt_msg.header)
except CvBridgeError as e: except CvBridgeError as e:
print(e) print(e)
...@@ -148,8 +158,7 @@ class human_detector: ...@@ -148,8 +158,7 @@ class human_detector:
# prediction_level_list.append(scores[0][0]) # prediction_level_list.append(scores[0][0])
def main(args): def main():
print(args)
_ = human_detector() _ = human_detector()
rospy.init_node('human_detector', anonymous=True) rospy.init_node('human_detector', anonymous=True)
rospy.spin() rospy.spin()
...@@ -157,4 +166,34 @@ def main(args): ...@@ -157,4 +166,34 @@ def main(args):
if __name__ == '__main__': if __name__ == '__main__':
main(sys.argv) print('Current Tensorflow Version: ' + str(tf.__version__))
# ospy.set_param('use_sim_time', 'True')
main()
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment