SVCAM_examples/finding_obstacles.py at main · AppliedRobotics/SVCAM_examples · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
#!/usr/bin/env python3
from __future__ import print_function
import sys
import json
import numpy as np
import message_filters
import rospy
from std_msgs.msg import Bool
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
# import plc

class get_distance:
    def __init__(self):
        self.camera_info_sub = message_filters.Subscriber('/trackingcam3d_client_ros/trackingcam3d0/left/camera_info', CameraInfo)
        self.depth_sub = message_filters.Subscriber("/trackingcam3d_client_ros/trackingcam3d0/depth/image_rect",Image)

        self.ts = message_filters.ApproximateTimeSynchronizer([self.depth_sub, self.camera_info_sub], queue_size=10, slop=0.5)
        self.ts.registerCallback(self.callback)

        self.pub = rospy.Publisher('move', Bool, queue_size =1)
        self.pub_s = rospy.Publisher('move_s', Bool, queue_size =1)


    def callback(self, depth_data, camera_info):
        try:
            self.pub_s.publish((Bool(True)))
            camera_info_K = np.array(camera_info.K)
            width = 320#depth_data.width
            height = 480#depth_data.height

            PointCloud = []
            for uy in range(160, 320, 1):
                for ux in range(105, 210, 1):
                    z = (depth_data.data[uy*width + ux])
                    x = ux
                    y = uy
                    array = [x, y, z]
                    PointCloud.append(array)
            pc = np.array(PointCloud)
            sum = 0
            n = 0

            for i in range(len(PointCloud)):

                if pc[i, 2] != 0:
                    sum = sum + pc[i, 2]
                    n = n+1
            if n >0:
                k = sum/n

                if k>120:
                    self.pub.publish(Bool(True))
                else:
                    self.pub.publish(Bool(False))
            else:
                self.pub.publish(Bool(False))
        except CvBridgeError as e:
            self.pub_s.publish((Bool(False)))
            print(e)

def main(args):
    rospy.init_node('check_obstacle')
    fd = get_distance()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")

if __name__ == '__main__':
    main(sys.argv)