Skip to content
Snippets Groups Projects
Commit 2572ae01 authored by jerry's avatar jerry
Browse files

remove .pyc files

parent 8c26e96d
No related branches found
No related tags found
No related merge requests found
Showing
with 58 additions and 14 deletions
.idea .idea
*.bag *.bag
*.pyc
.catkin_workspace .catkin_workspace
# *CMakeLists.txt # *CMakeLists.txt
\ No newline at end of file
...@@ -4,6 +4,7 @@ Human detection model that runs on ROS ...@@ -4,6 +4,7 @@ Human detection model that runs on ROS
This is a human detection mode that uses Intel RealSense D435 Camera and ROS Kinetic. Visit the GitHub page of This is a human detection mode that uses Intel RealSense D435 Camera and ROS Kinetic. Visit the GitHub page of
Intel RealSense-ROS for more camera installation. Intel RealSense-ROS for more camera installation.
## Installing The Workspace
Run the following commands on terminal to get started. Run the following commands on terminal to get started.
First we start a workspace First we start a workspace
...@@ -21,3 +22,22 @@ source ./devel/setup.bash ...@@ -21,3 +22,22 @@ source ./devel/setup.bash
``` ```
*Something like that. I will fix it later.* *Something like that. I will fix it later.*
## Sample Data Files
Below are some links to example files. These bag files are too big for GitHub so I put them on Google Drive.
Download them as you wish. Here are the types and the message topics.
I recorded them with Intel RealSense D435 Camera at 60 fps (I think).
```
types: sensor_msgs/Image [060021388200f6f0f447d0fcd9c64743]
sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181]
topics: /camera/color/image_raw 534 msgs : sensor_msgs/Image
/camera/depth/image_rect_raw 534 msgs : sensor_msgs/Image
/camera/depth_registered/points 525 msgs : sensor_msgs/PointCloud2
```
1. Indoor settings
- *Will update as I go*
2. Outdoor settings
- *Will update as I go*
\ No newline at end of file
import numpy.ma as ma
import cv2
def filter_background(roi):
ret_val = ma.masked_less(roi, 2900)
# roi[roi <= 2900]
return ret_val
def mask_the_thing(image, mask):
cv2.bitwise_and(image, mask)
...@@ -6,12 +6,11 @@ import cv2 ...@@ -6,12 +6,11 @@ import cv2
import rospy import rospy
import message_filters import message_filters
sys.path.append('/home/jerry/Documents/workspaces/ROS_human_detection/src/human_detection')
from visualization_msgs.msg import Marker from visualization_msgs.msg import Marker
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 from human_detection.msg import box_list
from depth_filter import *
text_position = (10, 50) text_position = (10, 50)
font = cv2.FONT_HERSHEY_SIMPLEX font = cv2.FONT_HERSHEY_SIMPLEX
...@@ -19,43 +18,48 @@ font_scale = 0.75 ...@@ -19,43 +18,48 @@ font_scale = 0.75
font_color = (255, 255, 255) font_color = (255, 255, 255)
line_type = 2 line_type = 2
class obj_dist: class obj_dist:
def __init__(self): def __init__(self):
# Initializing ROS Topics # Initializing ROS Topics
self.bridge = CvBridge() self.bridge = CvBridge()
self.dist_pub = rospy.Publisher('/obj_to_dist/human_distance', Image, queue_size=10) self.dist_pub = rospy.Publisher('/obj_to_dist/human_distance', Image, queue_size=10)
self.render_pub = rospy.Publisher('/obj_to_dist/render_human', Marker, queue_size=10) self.render_pub = rospy.Publisher('/obj_to_dist/render_human', Marker, queue_size=10)
self.roi_pub = rospy.Publisher('/obj_to_dist/disappera', 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.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.human_image_sub = message_filters.Subscriber('/human_detected_image/image', Image)
self.depth_image_sub = message_filters.Subscriber('/camera/depth/image_rect_raw', Image) self.depth_image_sub = message_filters.Subscriber('/camera/depth/image_rect_raw', Image)
self.pcl_sub = message_filters.Subscriber('/camera/depth_registered/points', PointCloud2) # self.pcl_sub = message_filters.Subscriber('/camera/depth_registered/points', PointCloud2)
ts = message_filters.ApproximateTimeSynchronizer([self.bbx_sub, self.human_image_sub, ts = message_filters.ApproximateTimeSynchronizer([self.bbx_sub, self.human_image_sub,
self.depth_image_sub, self.pcl_sub], self.depth_image_sub],
queue_size=10, queue_size=10,
slop=1) slop=1)
ts.registerCallback(self.callback) ts.registerCallback(self.callback)
def callback(self, bbx, image, depth, point_cloud): def callback(self, bbx, image, depth):
if bbx.length: if bbx.length:
cv_depth = self.bridge.imgmsg_to_cv2(depth, 'passthrough') cv_depth = self.bridge.imgmsg_to_cv2(depth, 'passthrough')
cv_image = self.bridge.imgmsg_to_cv2(image, 'bgr8') cv_image = self.bridge.imgmsg_to_cv2(image, 'bgr8')
for box in bbx.people_list: for box in bbx.people_list:
cood = self.get_human_distance(cv_depth, cv_image, box) cood = self.get_human_distance(cv_depth, cv_image, box)
self.render_human(cood, point_cloud) self.render_human(cood)
def get_human_distance(self, cv_depth, cv_image, box): def get_human_distance(self, cv_depth, cv_image, box):
roi_depth = cv_depth[box.xmin:box.xmax, box.ymin:box.ymax] roi_depth = cv_depth[box.ymin:box.ymax, box.xmin:box.xmax]
filtered_depth = roi_depth[roi_depth <= 2900] jk = np.shape(roi_depth)
jk = jk[0]*jk[1]
filtered_depth = filter_background(roi_depth)
_size = len(filtered_depth) _size = len(filtered_depth)
print('size of original: {}'.format(jk))
print('size of filtered: {}'.format(_size))
print('')
avg_distance = filtered_depth.sum() / _size / 1000 avg_distance = filtered_depth.sum() / _size / 1000
x_meter = self.get_x_in_meters(box.xmin, box.xmax, avg_distance) x_meter = self.get_x_in_meters(box.xmin, box.xmax, avg_distance)
cv2.putText(cv_image, '{} meters'.format(avg_distance), cv2.putText(cv_image, '{} meters'.format(avg_distance),
(box.xmin, box.ymax - 100), (box.xmin, box.ymax - 100),
...@@ -63,10 +67,14 @@ class obj_dist: ...@@ -63,10 +67,14 @@ class obj_dist:
font_scale, font_scale,
font_color, font_color,
line_type) line_type)
# self.roi_pub.publish(self.bridge.cv2_to_imgmsg(filtered_depth.data))
self.roi_pub.publish(self.bridge.cv2_to_imgmsg(roi_depth))
self.dist_pub.publish(self.bridge.cv2_to_imgmsg(cv_image)) self.dist_pub.publish(self.bridge.cv2_to_imgmsg(cv_image))
return x_meter, 0, avg_distance return x_meter, 0, avg_distance
def render_human(self, cood, point_cloud): def render_human(self, cood):
marker = Marker() marker = Marker()
marker.header.frame_id = 'camera_color_optical_frame' marker.header.frame_id = 'camera_color_optical_frame'
marker.header.stamp = rospy.Time.now() marker.header.stamp = rospy.Time.now()
...@@ -103,6 +111,7 @@ class obj_dist: ...@@ -103,6 +111,7 @@ class obj_dist:
def main(args): def main(args):
rospy.init_node('obj_dist', anonymous=True) rospy.init_node('obj_dist', anonymous=True)
print("object_to_distance.py is running")
_ = obj_dist() _ = obj_dist()
rospy.spin() rospy.spin()
cv2.destroyAllWindows() cv2.destroyAllWindows()
......
File deleted
File deleted
File deleted
File deleted
File deleted
File deleted
File deleted
File deleted
File deleted
File deleted
File deleted
File deleted
File deleted
File deleted
File deleted
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