From a7dd3a85dd4cb0015651806462cc2bd84628d3a8 Mon Sep 17 00:00:00 2001 From: "Vaibhav.hariani@gmail.com" Date: Wed, 10 Apr 2024 18:02:43 -0400 Subject: [PATCH] The U.I Lives! Squashed all existing bugs (As far as I'm aware), meaning that multiple topics are subscribed too, being rendered, and without fail or concern. This was fixed with a spin_once and then controlling the spin speed manually, rather than letting RCLPY do it. In addition, adding a delay to the system helps solve some issues involving tab switching too fast. --- .../pages/\360\237\233\243 Lane_Detection.py" | 53 +++++++++++-------- ros2/src/lanefollowingtest/cam_publisher.py | 23 ++++---- 2 files changed, 43 insertions(+), 33 deletions(-) diff --git "a/ros2/UI/pages/\360\237\233\243 Lane_Detection.py" "b/ros2/UI/pages/\360\237\233\243 Lane_Detection.py" index 837d1e090..1700568da 100644 --- "a/ros2/UI/pages/\360\237\233\243 Lane_Detection.py" +++ "b/ros2/UI/pages/\360\237\233\243 Lane_Detection.py" @@ -5,6 +5,8 @@ from rclpy.node import Node from rclpy.qos import qos_profile_sensor_data from cv_bridge import CvBridge +import time + class Image_Handler(Node): @@ -14,11 +16,17 @@ def __init__(self): self._frame_containers = None super().__init__('Streamlit_Image_Handler') self._bridge = CvBridge() - self.camData_subscriber = self.create_subscription( + self.imgData_subscriber = self.create_subscription( Image, '/camera/image', self.camData_callback, qos_profile_sensor_data) + self.hsvData_subscriber = self.create_subscription( + Image, + '/camera/image_hsv', + self.hsvData_callback, + qos_profile_sensor_data) + def tabs(self,tabs): self._tabs = tabs @@ -30,16 +38,12 @@ def camData_callback(self, msg_in): raw = msg_in img = self._bridge.imgmsg_to_cv2(raw) self._frame_containers[0].image(img) - st.write("IMAGE RECIEVED") -@st.cache_data -def startup(): - rclpy.init() - return None + def hsvData_callback(self, msg_in): + raw = msg_in + img = self._bridge.imgmsg_to_cv2(raw) + self._frame_containers[1].image(img) -def render(): - if("checkbox" not in st.session_state or st.session_state["checkbox"]): - # See ../../ros2/src/lanefollowingtest/LaneDetection.py # This page should render ALL images in there, as well as publish @@ -47,22 +51,27 @@ def render(): st.set_page_config( page_title="Lane Detection", page_icon="🛣") - startup() - - st.session_state["checkbox"] = st.checkbox("Render Video Feed",callback = render) + time.sleep(0.2) + render = st.checkbox("Render Video Feed") st.write( "This should render all of the images related to Lane Detection, and relevant parameters.") tabs = st.tabs(["Original", "HSV", "Sliding Windows"]) if "node" not in st.session_state: - handler = Image_Handler() - handler.tabs(tabs) - st.session_state["node"] = handler - if(render): - rclpy.spin(st.session_state['node']) - st.session_state['node'].destroy_node() - rclpy.shutdown() - - # I Don't even know if we'll want this, but it's a nice to have anyway - Restart = st.button("ESTOP", type="primary") \ No newline at end of file + try: + rclpy.init() + except RuntimeError: + st.warning("something went wrong performance may be degraded. Try restarting fully") + finally: + handler = Image_Handler() + handler.tabs(tabs) + st.session_state["node"] = handler + if render: + while(True): + try: + rclpy.spin_once(st.session_state['node']) + time.sleep(0.01) + except: + st.warning("Something went wrong, perhaps tabs were clicked too quickly? Try restarting.") + break diff --git a/ros2/src/lanefollowingtest/cam_publisher.py b/ros2/src/lanefollowingtest/cam_publisher.py index 82ef40e01..e5b21f2e5 100644 --- a/ros2/src/lanefollowingtest/cam_publisher.py +++ b/ros2/src/lanefollowingtest/cam_publisher.py @@ -11,7 +11,7 @@ def __init__(self): self.logger = self.get_logger() # Set desired frame rate (may need adjustment based on hardware) - self.frame_rate = 60 + self.frame_rate = 30 # Initialize OpenCV video capture object self.cap = cv2.VideoCapture("/dev/video0") # 0 for default webcam @@ -26,11 +26,11 @@ def __init__(self): # Create image publisher self.image_pub = self.create_publisher( - Image, '/camera/image', 10) # Adjust queue size if needed + Image, '/camera/image', 5) # Adjust queue size if needed - # Create image publisher + # # Create image publisher self.hsv_pub = self.create_publisher( - Image, '/camera/image_hsv', 10) # Adjust queue size if needed + Image, '/camera/image_hsv', 5) # Adjust queue size if needed # Timer to capture and publish images @@ -45,14 +45,15 @@ def capture_and_publish(self): if not ret: self.logger.warn("Failed to capture image!") return - + raw_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB) + raw_image = cv2.resize(raw_image, (640,480)) + raw_image_msg = self.bridge.cv2_to_imgmsg(raw_image, "bgr8") # Convert OpenCV image to ROS image message - raw_image = self.bridge.cv2_to_imgmsg(cv_image, encoding="passthrough") - hsv_img = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV) - hsv_image = self.bridge.cv2_to_imgmsg(hsv_img, encoding="passthrough") - # Publish the image - self.image_pub.publish(raw_image) - self.hsv_pub.publish(hsv_image) + hsv_image = cv2.cvtColor(raw_image, cv2.COLOR_BGR2HSV) + hsv_image_msg = self.bridge.cv2_to_imgmsg(hsv_image, encoding="passthrough") + # # Publish the image + self.image_pub.publish(raw_image_msg) + self.hsv_pub.publish(hsv_image_msg) def main():