diff --git a/.gitignore b/.gitignore
index 51d199d680ad6c32f552b59f4ca6bc7319d3ef66..7799cff16dbad040a0dd311906f8aa7ae049d598 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,4 @@
-/.idea
-.bag
-.txt
+.idea
+*.bag
+*CMakeLists.txt
.catkin_workspace
diff --git a/get_obj_dist/src/depth_cam.py b/get_obj_dist/src/depth_cam.py
new file mode 100755
index 0000000000000000000000000000000000000000..e48b8d5bfa90c04b6124b7f415c245601c8ecf9e
--- /dev/null
+++ b/get_obj_dist/src/depth_cam.py
@@ -0,0 +1,69 @@
+#!/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)
diff --git a/get_obj_dist/src/object_to_distance.py b/get_obj_dist/src/object_to_distance.py
index 1dbc6bd90715e6101ba18bed1a650719dfd557d0..3070d152e3bb50eab0a9b002cb786797c9c230a4 100755
--- a/get_obj_dist/src/object_to_distance.py
+++ b/get_obj_dist/src/object_to_distance.py
@@ -3,35 +3,72 @@
import numpy as np
import sys
import cv2
-
import rospy
-import pyrealsense2 as rs2
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 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:
def __init__(self):
# Initializing ROS Topics
self.bridge = CvBridge()
- dist_pub = rospy.Publisher('/obj_to_dist', 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.dist_pub = rospy.Publisher('/obj_to_dist/human_distance', Image, queue_size=10)
+
+ # 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)
- def callback(self, Image, Depth):
- print('wtf')
+ def callback(self, bbx, image, depth):
+ 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):
- _ = obj_dist()
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)
-
diff --git a/get_obj_dist/src/rs_plc.py b/get_obj_dist/src/rs_plc.py
deleted file mode 100644
index 6a01833f5197132aa37dc83fd164f7090f71b6f3..0000000000000000000000000000000000000000
--- a/get_obj_dist/src/rs_plc.py
+++ /dev/null
@@ -1,53 +0,0 @@
-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
diff --git a/get_obj_dist/src/scene_process.py b/get_obj_dist/src/scene_process.py
deleted file mode 100755
index c570f851f9e4b7c21cb8f6861ed753da48a2fbef..0000000000000000000000000000000000000000
--- a/get_obj_dist/src/scene_process.py
+++ /dev/null
@@ -1,31 +0,0 @@
-#!/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()
diff --git a/get_obj_dist/src/true_color.py b/get_obj_dist/src/true_color.py
deleted file mode 100644
index a8d60525eca1df797ab6e974fb237612e8eae83e..0000000000000000000000000000000000000000
--- a/get_obj_dist/src/true_color.py
+++ /dev/null
@@ -1,14 +0,0 @@
-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
diff --git a/human_detection/msg/box_list.msg b/human_detection/msg/box_list.msg
new file mode 100644
index 0000000000000000000000000000000000000000..29697ac540a56c428394db91f06464631630b05f
--- /dev/null
+++ b/human_detection/msg/box_list.msg
@@ -0,0 +1,3 @@
+std_msgs/Header header
+int8 length
+bounding_box[] people_list
\ No newline at end of file
diff --git a/human_detection/src/analysis_tools/define.pyc b/human_detection/src/analysis_tools/define.pyc
index 83dfaef6410ca36967ee2f71b3a310cc3aa32192..f56fc3a6ed9894a444dcdadfa3a44b25d16ba6cd 100644
Binary files a/human_detection/src/analysis_tools/define.pyc and b/human_detection/src/analysis_tools/define.pyc differ
diff --git a/human_detection/src/reference.py b/human_detection/src/reference.py
index 69df44abacf50c2561f2d6c0b60122636eb44973..a94bb1d56fd4ea924a1e152cb0fa9e322f399987 100755
--- a/human_detection/src/reference.py
+++ b/human_detection/src/reference.py
@@ -31,9 +31,9 @@ class image_converter:
cv2.waitKey(3)
try:
- ret_val = self.bridge.cv2_to_imgmsg(cv_image, "bgr8")
- ret_val.header = data.header
- self.image_pub.publish(ret_val)
+ ret_img = self.bridge.cv2_to_imgmsg(cv_image, "bgr8")
+ ret_img.header = data.header
+ self.image_pub.publish(ret_img)
except CvBridgeError as e:
print(e)
diff --git a/human_detection/src/webcam.py b/human_detection/src/webcam.py
index db485039367ecf26f4d708ca643bb3c016456f29..e6b52e0172e4096bfc704f888ff952e20aab1177 100755
--- a/human_detection/src/webcam.py
+++ b/human_detection/src/webcam.py
@@ -4,15 +4,15 @@
import time
import sys
import os
+import rospy
import tensorflow as tf
+
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.
from analysis_tools.data_grapher 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 cv_bridge import CvBridge, CvBridgeError
@@ -21,7 +21,6 @@ from object_detection.utils import label_map_util
from object_detection.utils import visualization_utils as vis_util
# Defining Paths
-print('Current Tensorflow Version: ' + str(tf.__version__))
CWD_PATH = '/home/jerry/Documents/workspaces/ROS_human_detection/src/human_detection/src'
LABEL_MAPS = ['human_label_map.pbtxt', 'mscoco_label_map.pbtxt']
MODEL_NAME = 'ssd_mobilenet_v1_coco_2017_11_17'
@@ -68,7 +67,7 @@ class human_detector:
self.bridge = CvBridge()
self.image_pub = rospy.Publisher(
"/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(
"/camera/color/image_raw", Image, self.callback)
@@ -109,12 +108,12 @@ class human_detector:
min_score_thresh=0.60)
# Calculate frame time
- # t1 = time.time()
- # run_time = t1 - t0
- # if run_time < 1:
- # run_time_list.append(run_time)
- # if len(run_time_list) > 10:
- # del run_time_list[0]
+ t1 = time.time()
+ run_time = t1 - t0
+ if run_time < 1:
+ run_time_list.append(run_time)
+ if len(run_time_list) > 10:
+ del run_time_list[0]
if len(run_time_list) > 0:
avg_run_time = round(sum(run_time_list) / len(run_time_list) * 1000, 1)
@@ -124,21 +123,32 @@ class human_detector:
font_scale,
font_color,
line_type)
+
counter = 0
+ list_length = 0
+ bbx_list = box_list()
+ bbx_list.header.stamp = rospy.Time.now()
+
for score in scores[0]:
if score > 0.6:
+ list_length += 1
coordinates = bounding_box()
coordinates.ymin = int(boxes[0, counter, 0] * screen_height)
coordinates.xmin = int(boxes[0, counter, 1] * screen_width)
coordinates.ymax = int(boxes[0, counter, 2] * screen_height)
coordinates.xmax = int(boxes[0, counter, 3] * screen_width)
- self.bbx_pub.publish(coordinates)
+ bbx_list.people_list.append(coordinates)
counter += 1
+ print(list_length)
+ bbx_list.length = list_length
+ self.bbx_pub.publish(bbx_list)
+
try:
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)
+ # print(cvt_msg.header)
except CvBridgeError as e:
print(e)
@@ -148,8 +158,7 @@ class human_detector:
# prediction_level_list.append(scores[0][0])
-def main(args):
- print(args)
+def main():
_ = human_detector()
rospy.init_node('human_detector', anonymous=True)
rospy.spin()
@@ -157,4 +166,34 @@ def main(args):
if __name__ == '__main__':
- main(sys.argv)
+ print('Current Tensorflow Version: ' + str(tf.__version__))
+ # ospy.set_param('use_sim_time', 'True')
+ main()
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+