initial commit

Author: Takaki

.gitignore

@@ -0,0 +1,7 @@
+videos/
+pics/
+results/
+*.avi
+*.mp4
+*.pyc
+*~

README.md

@@ -0,0 +1,11 @@
+# Traffic Signal Detection
+
+```
+$ roslaunch single_camera_streaming.launch device:=/dev/video1
+```
+```
+$ python signal_publisher.py
+```
+```
+python signal_subscriber.py
+```

detection_class.py

@@ -0,0 +1,141 @@
+import cv2
+import numpy as np
+import os
+import copy
+
+class SignalDetecor:
+
+    #generating diff image
+    def diffimg(self, after, before):
+
+        blue_before  = before[:,:,0]
+        green_before = before[:,:,1]
+        red_before   = before[:,:,2]
+
+        blue_after  = after[:,:,0]
+        green_after = after[:,:,1]
+        red_after   = after[:,:,2]
+
+        blue_final  = blue_after.astype(np.int32) - blue_before.astype(np.int32) + 255
+        green_final = green_after.astype(np.int32)- green_before.astype(np.int32) + 255
+        red_final   = red_after.astype(np.int32) - red_before.astype(np.int32) + 255
+
+        output=np.zeros((after.shape[0],after.shape[1],3))
+        output[:,:,0]=blue_final
+        output[:,:,1]=green_final
+        output[:,:,2]=red_final
+
+        return (output/2.0).astype(np.uint8)
+
+
+    def __init__(self):
+
+        # load template images
+        self.temp0 = cv2.imread('templates/red2green.png')
+        self.temp1 = cv2.imread('templates/red2black.png')
+        self.temp2 = cv2.imread('templates/black2green.png')
+        self.temp3 = cv2.imread('templates/green2red.png')
+
+        self.temp0_h = self.temp0.shape[0]
+        self.temp0_w = self.temp0.shape[1]
+        self.temp3_h = self.temp3.shape[0]
+        self.temp3_w = self.temp3.shape[1]
+
+        self.frame4=0
+        self.frame3=0
+        self.frame2=0
+        self.frame1=0
+
+        self.ok_desu=0
+        self.output=np.zeros((600,600,3))
+
+        self.green_cnt=0
+        self.red_cnt=0
+
+        # threshold of template matching
+        self.green_th = 0.65 #stable:0.65
+        self.red_th = 0.7 #stable:0.7
+
+        # number of detections
+        self.nd = 3
+
+        # signal states (0:unkown, 1:green, 2:red)
+        self.state = 0
+
+
+    def __call__(self, frame):
+
+        new_frame = frame
+        display_frame = copy.deepcopy(new_frame)
+
+        if self.ok_desu>4:
+
+            output=self.diffimg(new_frame,self.frame4)
+            result0=cv2.matchTemplate(output,self.temp0,method=cv2.TM_CCOEFF_NORMED)
+            result1=cv2.matchTemplate(output,self.temp1,method=cv2.TM_CCOEFF_NORMED)
+            result2=cv2.matchTemplate(output,self.temp2,method=cv2.TM_CCOEFF_NORMED)
+            result3=cv2.matchTemplate(output,self.temp3,method=cv2.TM_CCOEFF_NORMED)
+
+            mmlr0=cv2.minMaxLoc(result0)
+            mmlr1=cv2.minMaxLoc(result1)
+            mmlr2=cv2.minMaxLoc(result2)
+            mmlr3=cv2.minMaxLoc(result3)
+
+            # condition1: RED to GREEN
+            if mmlr0[1]>self.green_th and mmlr0[1] > mmlr2[1]:
+                self.green_cnt+=1
+            else:
+                self.green_cnt=0
+
+            if self.green_cnt==self.nd:
+                self.state = 1
+                print "The signal turned from RED to GREEN !"
+                cv2.rectangle(display_frame,
+                              (mmlr0[3][0],mmlr0[3][1]),
+                              (mmlr0[3][0]+temp0_w,mmlr0[3][1]+temp0_h),
+                              (0,255,0),3)
+                self.green_cnt=0
+
+            # condition2: GREEN to RED
+            if mmlr3[1]>self.red_th and mmlr3[1] > mmlr1[1]:
+                self.red_cnt+=1
+            else:
+                self.red_cnt=0
+
+            if self.red_cnt==self.nd:
+                self.state = 2
+                print "The signal turned from GREEN to RED !"
+                cv2.rectangle(display_frame,
+                              (mmlr3[3][0],mmlr3[3][1]),
+                              (mmlr3[3][0]+temp3_w,mmlr3[3][1]+temp3_h),
+                              (0,0,255),3)
+                self.red_cnt = 0
+
+            # display the signal state
+            if self.state == 0:
+                signal_state = "UNKOWN"
+                color = (0,0,0)
+            elif self.state == 1:
+                signal_state = "GREEN"
+                color = (0,255,0)
+            else:
+                signal_state = "RED"
+                color = (0,0,255)
+
+            message = "signal state "+str(self.state)+": "+signal_state
+            #print message
+            cv2.putText(display_frame, signal_state, (10, 25),
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2)
+
+            # display the resulting frame
+            cv2.imshow('frame',display_frame)
+            cv2.imshow('diff4',output)
+            cv2.waitKey(3)
+
+        self.frame4=self.frame3
+        self.frame3=self.frame2
+        self.frame2=self.frame1
+        self.frame1=new_frame
+        self.ok_desu+=1
+
+        return self.state

signal_publisher.py

@@ -0,0 +1,35 @@
+import rospy
+from std_msgs.msg import Int8
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+import cv2
+
+from detection_class import *
+
+class SignalDetecorNode:
+
+    def __init__(self):
+        rospy.init_node('signal_detector', anonymous=True)
+        self.bridge = CvBridge()
+        self.image_sub = rospy.Subscriber("center_cam/image_raw",Image,self.callback)
+        self.signal_pub = rospy.Publisher('signal_info', Int8, queue_size=10)
+        self.detection = SignalDetecor()
+
+    def callback(self,data):
+        try:
+            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
+            state = self.detection(cv_image)
+            self.signal_pub.publish(state)
+            rospy.loginfo(str(state)+" is published.")
+        except CvBridgeError as e:
+            print(e)
+
+def main():
+    SignalDetecorNode()
+    try:
+        rospy.spin()
+    except KeyboardInterrupt:
+         print("Shutting down")
+
+if __name__ == '__main__':
+    main()

signal_subscriber.py

@@ -0,0 +1,24 @@
+import rospy
+from std_msgs.msg import Int8
+
+def callback(data):
+
+    if data.data == 0:
+        signal_state = "UNKOWN"
+    elif data.data == 1:
+        signal_state = "GREEN"
+    else:
+        signal_state = "RED"
+
+    message = " signal state : "+signal_state
+    rospy.loginfo(rospy.get_caller_id() + '%s', message)
+
+
+def signal_subscriber():
+
+    rospy.init_node('signal_subscriber', anonymous=True)
+    rospy.Subscriber('signal_info', Int8, callback)
+    rospy.spin()
+
+if __name__ == '__main__':
+    signal_subscriber()

single_camera_streaming.launch

@@ -0,0 +1,12 @@
+<launch>
+  <node name="center_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
+    <!-- <param name="video_device" value="/dev/video0" /> -->
+    <param name="video_device" value="$(arg device)" />
+    <param name="image_width" value="320" />
+    <param name="image_height" value="240" />
+    <param name="framerate" value="20" />
+    <param name="pixel_format" value="yuyv" />
+    <param name="camera_frame_id" value="usb_cam" />
+    <param name="io_method" value="mmap"/>
+  </node>
+</launch>