diff --git a/hefestus/hefestus_perception/src/lane_detection.py b/hefestus/hefestus_perception/src/lane_detection.py index 5473e79..9a8f1d2 100755 --- a/hefestus/hefestus_perception/src/lane_detection.py +++ b/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 diff --git a/hefestus/hefestus_sensing/src/main_vision.py b/hefestus/hefestus_sensing/src/main_vision.py index c41e597..1101310 100755 --- a/hefestus/hefestus_sensing/src/main_vision.py +++ b/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: diff --git a/hefestus/hefestus_sensing/src/sensing.py b/hefestus/hefestus_sensing/src/sensing.py index a091632..ceee713 100755 --- a/hefestus/hefestus_sensing/src/sensing.py +++ b/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