#2 Fixed some problems

hefestus/hefestus_perception/src/lane_detection.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/usr/bin/env python
 
 import cv2
 import numpy as np

hefestus/hefestus_sensing/src/main_vision.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/usr/bin/env python
 import rospy
 import cv2
 from sensor_msgs.msg import Image
@@ -9,16 +9,31 @@ def mainVisionNode():
     img_bridge = CvBridge()
 
     cam = cv2.VideoCapture(2)
+
+    if not cam.isOpened():
+        rospy.logerr("Camera could not be opened.")
+        return
 
-    rate = rospy.Rate(1) # 10hz
+    rate = rospy.Rate(1) # 1hz
 
-    while not rospy.is_shutdown():
-        _, img_raw = cam.read()  
-
-        img_message = img_bridge.cv2_to_imgmsg(img_raw, "bgr8")
+    try:
+        while not rospy.is_shutdown():
+            ret, img_raw = cam.read()  
 
-        img_Pub.publish(img_message)
-        rate.sleep()
+            if not ret:
+                rospy.logerr("Failed to capture image")
+                break
+
+            img_message = img_bridge.cv2_to_imgmsg(img_raw, "bgr8")
+            img_Pub.publish(img_message)
+            rate.sleep()
+
+    except rospy.ROSInterruptException:
+        rospy.loginfo("ROS Interrupt Exception occurred.")
+
+    finally:
+        cam.release()
+        rospy.loginfo("Camera released.")
 
 if __name__ == "__main__":    
     try:

hefestus/hefestus_sensing/src/sensing.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/usr/bin/env python
 import cv2
 from cv_bridge import CvBridge, CvBridgeError
 import rospy