testDenseOpticalFlow.py 6.36 KB
Newer Older
1 2
#!/usr/bin/env python
"""
jad's avatar
jad committed
3
This node will is for testing the optical flow in order to detect opened doors
4 5 6 7 8 9 10
"""

import numpy as np
import cv2
import rospy
from sensor_msgs.msg import CompressedImage
from sensor_msgs.msg import Image
jad's avatar
jad committed
11
from std_msgs.msg import Float32,Int32
12 13 14
import sys
import math
from cv_bridge import CvBridge, CvBridgeError
jad's avatar
jad committed
15
import random
jad's avatar
jad committed
16
from nav_msgs.msg import Odometry
jad's avatar
jad committed
17
from activation_class import NodeActivate,returnResp
jad's avatar
jad committed
18
import time
19

jad's avatar
jad committed
20
class EnterDoors(NodeActivate,returnResp):
21
    def __init__(self):
jad's avatar
jad committed
22
        super(EnterDoors,self).__init__("checkDoors")
23
        self.br = CvBridge()
jad's avatar
jad committed
24 25 26
        self.prvs = None
        self.next = None
        self.hsv = None
jad's avatar
jad committed
27
        self.plot_image = None
jad's avatar
jad committed
28 29
        self.old_dat = None
        self.alpha = 0.25
jad's avatar
jad committed
30 31
        self.alpha2 = 0.35
        self.thresh = 200
jad's avatar
jad committed
32
        self.dat_bin = None
jad's avatar
jad committed
33
        self.my_vel = 0
jad's avatar
jad committed
34
        self.conf = 0
jad's avatar
jad committed
35 36 37 38 39 40 41 42 43 44

        self.img_read = rospy.Subscriber("/bebop/image_raw/compressed",CompressedImage,self.transform_image)
        self.img_pub = rospy.Publisher("/image_graph",Image,queue_size=1)
        self.roi_pub = rospy.Publisher("/image_roi",Image,queue_size=1)
        
        self.vel_sub = rospy.Subscriber("/bebop/odom",Odometry,self.update_vel)
        
        self.vel_pub_x = rospy.Publisher("/vel_in_x",Float32,queue_size=1)
        self.vel_pub_y = rospy.Publisher("/vel_in_y",Float32,queue_size=1)

jad's avatar
jad committed
45 46
        self.door_pub = rospy.Publisher("/door_det",Int32,queue_size=1)
        self.y_vel = 0
jad's avatar
jad committed
47
        self.act_time = -1
jad's avatar
jad committed
48
    def update_vel(self,ros_data):
jad's avatar
jad committed
49
        if(self.node_active == False):
jad's avatar
jad committed
50
            self.act_time = time.time()
jad's avatar
jad committed
51
            return
jad's avatar
jad committed
52 53
        twist = ros_data.twist.twist
        self.my_vel = np.absolute(twist.linear.y)
jad's avatar
jad committed
54 55 56
        curr_pose = ros_data.pose.pose
        cur_ang_quat = curr_pose.orientation.z
        if(cur_ang_quat > 0):
jad's avatar
jad committed
57
            self.y_vel = -0.4
jad's avatar
jad committed
58
        else:
jad's avatar
jad committed
59
            self.y_vel = 0.4
jad's avatar
jad committed
60
        
61
    def transform_image(self,ros_data):
jad's avatar
jad committed
62
        if(self.node_active == False):
jad's avatar
jad committed
63
            self.act_time = time.time()
jad's avatar
jad committed
64
            return
jad's avatar
jad committed
65
        self.vel_pub_x.publish(0.0)
jad's avatar
jad committed
66
        self.vel_pub_y.publish(self.y_vel)
jad's avatar
jad committed
67
        cv2_image = self.br.compressed_imgmsg_to_cv2(ros_data)
jad's avatar
jad committed
68
        cv2_image = cv2.resize(cv2_image, (320,240), interpolation = cv2.INTER_AREA)
jad's avatar
jad committed
69
        cv2_roi = cv2_image[80:160,:]
jad's avatar
jad committed
70
        cv2_gray = cv2.cvtColor(cv2_image,cv2.COLOR_BGR2GRAY)
jad's avatar
jad committed
71 72
        cv2_gray_roi = cv2_gray[80:160,:]
        self.hsv = np.zeros_like(cv2_roi)
jad's avatar
jad committed
73 74
        self.plot_image = np.ones_like(cv2_gray)
        self.plot_image = 255*self.plot_image
jad's avatar
jad committed
75
        if(self.prvs is None):
jad's avatar
jad committed
76
            self.prvs = cv2_gray_roi
77
        else:
jad's avatar
jad committed
78
            self.next = cv2_gray_roi
jad's avatar
jad committed
79 80
            self.draw_and_dispaly() 
            self.prvs = self.next.copy()
jad's avatar
jad committed
81 82 83 84 85
    def check_doors(self,limits_idx):
        for i in range(len(limits_idx) - 1):
            left = limits_idx[i]
            right = limits_idx[i+1]
            img = self.next[...,left:right]
jad's avatar
jad committed
86
            #print img.shape
87
            height, width = img.shape
jad's avatar
jad committed
88
            if self.dat_bin[int((right-1))] == 0:
jad's avatar
jad committed
89
                #In this case we have no optical flow, we need to compute the variance
90
                myrands_x = random.sample(xrange(width),width)
jad's avatar
jad committed
91
                #myrands_y = random.sample(xrange(height),10)
jad's avatar
jad committed
92
                tmp_pixels_x = img[...,myrands_x]
jad's avatar
jad committed
93
                #tmp_pixels_y = img[myrands_y,...]
jad's avatar
jad committed
94
                var_x = np.var(tmp_pixels_x,axis = 0)
jad's avatar
jad committed
95
                #var_y = np.var(tmp_pixels_y,axis = 1)
jad's avatar
jad committed
96
                m_var_x = np.mean(var_x)
jad's avatar
jad committed
97
                #m_var_y = np.mean(var_y)
jad's avatar
jad committed
98
                if(m_var_x > 500 and abs(time.time() - self.act_time) > 3):
jad's avatar
jad committed
99 100
                    #print "Door detected between "+str(left) + " and "+str(right)
                    cv2.circle(self.plot_image,(int((left+right)/2),100),3,(0,0,0),-1)
jad's avatar
jad committed
101 102 103 104
                    if(left < 20 and right > 300):
                        self.conf = self.conf + 1
                    else:
                        self.conf = 0
jad's avatar
jad committed
105
                    if(self.conf > 3 ):
jad's avatar
jad committed
106
                        #Here we can say that, we have a door
jad's avatar
jad committed
107 108
                        #self.door_pub.publish(1)
                        self.send_conf()
jad's avatar
jad committed
109

jad's avatar
jad committed
110 111
    def draw_and_dispaly(self):
        flow = cv2.calcOpticalFlowFarneback(self.prvs,self.next, None, 0.5, 2, 15, 3, 5, 1.2, 0)
jad's avatar
jad committed
112 113 114
        u = flow[...,0]
        v = flow[...,1]
        mag, ang = cv2.cartToPolar(u, v)
jad's avatar
jad committed
115
        mag[mag < 2] = 0
jad's avatar
jad committed
116
        dat2 = np.absolute(u.mean(0))
jad's avatar
jad committed
117 118 119
        u = np.absolute(u)
        u = 190/u
        u = u * self.my_vel
jad's avatar
jad committed
120
        dat = u.mean(0)
jad's avatar
jad committed
121 122 123 124 125
        #dat = np.absolute(dat)
        dat = dat*8
        #dat2 = dat2*10
        dat = dat + 0.01
        #dat = 1/dat
jad's avatar
jad committed
126
        dat[dat>230] = 230
jad's avatar
jad committed
127 128 129 130 131
        for i in range(319):
            dat[i+1] = dat[i] - self.alpha*(dat[i] - dat[i+1])
        if(not self.old_dat is None):
            dat = self.old_dat - self.alpha2*(self.old_dat - dat)
        #print dat
jad's avatar
jad committed
132 133 134
        self.hsv[...,0] = ang*180/np.pi/2
        self.hsv[...,2] = cv2.normalize(mag,None,0,255,cv2.NORM_MINMAX)
        bgr = cv2.cvtColor(self.hsv,cv2.COLOR_HSV2BGR)
jad's avatar
jad committed
135 136
        #cv2.imshow('frame2',bgr)
        #cv2.imshow('frame',self.next)
jad's avatar
jad committed
137
        self.dat_bin = np.zeros_like(dat)
jad's avatar
jad committed
138
        self.dat_bin[dat < self.thresh] = 1
139
        diff_dat = np.diff(self.dat_bin)
jad's avatar
jad committed
140 141 142 143
        diff_dat = np.absolute(diff_dat)
        idx = np.where(diff_dat == 1)
        idx = np.concatenate(idx)
        idx = idx + 1
144 145
        idx = np.insert(idx,0,[0],axis = 0)
        idx = np.append(idx,320)
jad's avatar
jad committed
146
        #print idx
jad's avatar
jad committed
147
        self.check_doors(limits_idx=idx)
jad's avatar
jad committed
148 149 150 151 152 153 154 155 156 157 158 159 160
        for i in range(320):
            self.plot_image[self.thresh,:] = 150
            self.plot_image[:,idx[1:-1]] = 150
            self.plot_image[int(dat[i]),i] = 0
            self.plot_image[int(dat[i])+1,i] = 0
            self.plot_image[int(dat[i])+2,i] = 0
            #self.plot_image[int(dat2[i]),i] = 120
            #self.plot_image[int(dat2[i])+1,i] = 120
            #self.plot_image[int(dat2[i])+2,i] = 120
        cmprsmsg = self.br.cv2_to_imgmsg(self.plot_image)
        self.img_pub.publish(cmprsmsg)
        cmprsmsg = self.br.cv2_to_imgmsg(self.next)
        self.roi_pub.publish(cmprsmsg)
jad's avatar
jad committed
161
        self.old_dat = dat.copy()
162 163 164
def main(args):
    rospy.init_node('ShowPoints', anonymous=True)
    #rospy.init_node('send_command', anonymous=True)
jad's avatar
jad committed
165
    sc = EnterDoors()
jad's avatar
jad committed
166
    rospy.spin()
167 168 169
if __name__ == '__main__':
    main(sys.argv)

jad's avatar
jad committed
170
#export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/melodic/lib/parrot_arsdk