Finalized UI publisher (For some reason it was not committed before),…

… and tested the lane determination system (It works!)

ros2/UI/pages/Lane_Detection.py

@@ -8,59 +8,94 @@
 import time
 from std_msgs.msg import String
 
-## This class currently subscribes to some topics in cam_publisher, which I used to test. Please rewrite for lane Detection.
+# This class currently subscribes to some topics in cam_publisher, which I used to test. Please rewrite for lane Detection.
+
+
 class Image_Handler(Node):
     def __init__(self):
         self._tabs = None
         self._img = None
-        self._frame_containers = None
+        self._columns = []
+        self._frame_containers = []
         super().__init__('Streamlit_Image_Handler')
         self._bridge = CvBridge()
 
-        self.imgData_subscriber = self.create_subscription(
+        self.lane_state_pub = self.create_subscription(
+            String, "lane_state", self.lane_state_callback, qos_profile_sensor_data)
+
+        self.imgData_subscriber_1 = self.create_subscription(
             Image,
-            '/camera/image',
-            lambda msg: self.camData_callback(msg,(self._frame_containers[0][0], self._frame_containers[0][1])),
+            'raw_left',
+            lambda msg: self.camData_callback(
+                msg, self._frame_containers[0][0]),
             qos_profile_sensor_data)
-
-        self.hsvData_subscriber = self.create_subscription(
+        self.imgData_subscriber_2 = self.create_subscription(
             Image,
-            '/camera/image_hsv',
-            lambda msg: self.camData_callback(msg,(self._frame_containers[1][0], self._frame_containers[1][1])),
+            'raw_right',
+            lambda msg: self.camData_callback(
+                msg, self._frame_containers[0][1]),
+            qos_profile_sensor_data)
+        self.transformed_subscriber_1 = self.create_subscription(
+            Image,
+            'tf_left',
+            lambda msg: self.camData_callback(
+                msg, self._frame_containers[1][0]),
+            qos_profile_sensor_data)
+        self.transformed_subscriber_2 = self.create_subscription(
+            Image,
+            'tf_right',
+            lambda msg: self.camData_callback(
+                msg, self._frame_containers[1][1]),
+            qos_profile_sensor_data)
+        self.sliding_subscriber_1 = self.create_subscription(
+            Image,
+            'sliding_left',
+            lambda msg: self.camData_callback(
+                msg, self._frame_containers[2][0]),
+            qos_profile_sensor_data)
+        self.sliding_subscriber_1 = self.create_subscription(
+            Image,
+            'sliding_right',
+            lambda msg: self.camData_callback(
+                msg, self._frame_containers[2][1]),
             qos_profile_sensor_data)
 
     def tabs(self, tabs):
         self._tabs = tabs
-        self._frame_containers = [(tab.empty(),tab.empty()) for tab in self._tabs]
+        self._columns = [(tab.columns(2)) for tab in self._tabs]
+        for column in self._columns:
+            tabs = (column[0].empty(), column[1].empty())
+            self._frame_containers.append(tabs)
 
-    #self._frame_containers[] is a container corresponding to one of the tabs: You can create
-    def camData_callback(self, msg_in,args):
+    # self._frame_containers[] is a container corresponding to one of the tabs: You can create
+    def camData_callback(self, msg_in, args):
         raw = msg_in
         img = self._bridge.imgmsg_to_cv2(raw)
-        args[0].image(img)
-        args[1].image(img)
+        with args:
+            st.image(img)
+
+    def lane_state_callback(self, msg):
+        st.write(msg.data)
 
 
-#This class is your publisher: Flesh it out and integrate accordingly
+# This class is your publisher: Flesh it out and integrate accordingly
 class Publisher(Node):
     def __init__(self):
-            super().__init__('Streamlit_Button_Publisher')
-            self.logger = self.get_logger()
-            self.button_pub = self.create_publisher(String, '/streamlit/button',10)
+        super().__init__('Streamlit_Button_Publisher')
+        self.logger = self.get_logger()
+        self.button_pub = self.create_publisher(
+            String, '/streamlit/button', 10)
+
 
 def demo_publish():
     msg = String()
     msg.data = "Streamlit Button!"
-    st.session_state["pub_node"].button_pub.publish(msg)
-    st.success("Message Published!")
-
-
 
 
 # See ../../ros2/src/lanefollowingtest/LaneDetection.py
 # This page should render ALL images in there, as well as publish important update data
 if __name__ == "__main__":
-    #How we Configure the page
+    # How we Configure the page
     st.set_page_config(
         page_title="Lane Detection",
         page_icon="🛣")
@@ -69,10 +104,10 @@ def demo_publish():
     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"])
+    tabs = st.tabs(["Original", "Transformed", "Sliding Windows"])
 
-    #This hunk initializes the ROS2 nodes without breaking anything :)
-    #Should not need to be tuoched
+    # This hunk initializes the ROS2 nodes without breaking anything :)
+    # Should not need to be tuoched
     if "sub_node" not in st.session_state:
         try:
             rclpy.init()
@@ -83,19 +118,22 @@ def demo_publish():
             handler = Image_Handler()
             handler.tabs(tabs)
             st.session_state["sub_node"] = handler
-    if "pub_node" not in st.session_state:            
-            st.session_state["pub_node"] = Publisher()
+
+    if "pub_node" not in st.session_state:
+        st.session_state["pub_node"] = Publisher()
+
+    lane_state = st.empty()
 
-#This should also not need to be modified
+# This should also not need to be modified
     if render:
         while (True):
-            try:                
+            # try:
+            with lane_state:
                 rclpy.spin_once(st.session_state['sub_node'])
-                time.sleep(0.01)
-            except:
-                st.warning(
-                    "Something went wrong, perhaps tabs were clicked too quickly? Try restarting.")
-                break
+            # time.sleep(0.01)
+        # except:
+        #     st.warning(
+        #         "Something went wrong, perhaps tabs were clicked too quickly? Try restarting.")
+        #     break
     else:
-            st.button("Ros Topic Publisher Demo!",on_click=demo_publish)
-
+        st.button("Ros Topic Publisher Demo!", on_click=demo_publish)

ros2/UI/pages/Object_Detection.py

@@ -10,6 +10,6 @@ def image_source_renderer(image):
 
 if __name__ == "__main__":
     st.set_page_config(
-        page_title="Lane Detection",
+        page_title="Object Detection",
         page_icon="🛑")
 

ros2/src/lanefollowingtest/LaneDetection.py

@@ -7,13 +7,13 @@
 from cv_bridge import CvBridge
 import cv2
 import numpy as np
-
+from std_msgs.msg import String
 
 # Inputs from both cameras
-vidcap_left = cv2.VideoCapture("/dev/video0")
+vidcap_left = cv2.VideoCapture("/dev/video3")
 vidcap_left.set(3, 640)
 vidcap_left.set(4, 480)
-vidcap_right = cv2.VideoCapture("/dev/video2")
+vidcap_right = cv2.VideoCapture("/dev/video1")
 vidcap_right.set(3, 640)
 vidcap_right.set(4, 480)
 
@@ -165,6 +165,7 @@ def __init__(self):
         # Publisher for error from the lane lines relative to the camera FOV
         self.camData_publisher = self.create_publisher(
             Float64, '/cam_data', 10)
+        self.lane_pubs = self.create_publisher(String, "lane_state",10)
         if Lane_Follower.GUI:
             self._bridge = CvBridge()
             image_labels = ("raw_left", "raw_right", "tf_left",
@@ -174,9 +175,9 @@ def __init__(self):
 
     def img_publish(self, label, img_raw):
         if (self.GUI):
+            new_img = cv2.cvtColor(img_raw, cv2.COLOR_BGR2RGB)
             self._publishers[label].publish(
-                self._bridge.cv2_to_imgmsg(img_raw, encoding="passthrough"))
-#        cv2.imshow(label, img_raw)
+                self._bridge.cv2_to_imgmsg(new_img, encoding="passthrough"))
 
     def measure_position_meters(self, left, right):
         left_x_pos = 0
@@ -214,16 +215,16 @@ def measure_position_meters(self, left, right):
     def determine_lane_size(self, img):
         # Taking in both warped images, determine which lane line is the longer one, and ergo the "solid line"
         # This may struggle on turns, but might work depending: Will need to characterize
-        gray = cv2.split(img)[3]
         # TODO, parametrize all constants here for tweaking in the U.I
-        edges = cv2.Canny(gray, 50, 150)
+        edges = cv2.Canny(img, 50, 150)
         lines = cv2.HoughLinesP(edges, 1, np.pi/180, 100,
-                                minLineLength=100, MaxLineGap=5)
+                                minLineLength=100, maxLineGap=5)
         m_length = 0
-        for line in lines:
-            x1, y1, x2, y2 = line[0]
-            length = (x1 - x2) ^ 2 + (y1-y2) ^ 2
-            m_length = max(m_length, length)
+        if(lines is not None):        
+            for line in lines:
+                x1, y1, x2, y2 = line[0]
+                length = (x1 - x2) ^ 2 + (y1-y2) ^ 2
+                m_length = max(m_length, length)
         return m_length
 
     def timer_callback(self):
@@ -267,9 +268,14 @@ def timer_callback(self):
             print(pos)
             msg_out.data = pos
             self.camData_publisher.publish(msg_out)
-            if(left_buffer > right_buffer and left_buffer > 0):
+            msg = String()
+            if(left_buffer > right_buffer):
+                msg.data = "In Left lane"
+                self.lane_pubs.publish(msg)
                 self._Left_Lane = True
-            elif (right_buffer > 0):
+            elif (right_buffer > left_buffer):
+                msg.data = "In Right lane"
+                self.lane_pubs.publish(msg)
                 self._Left_Lane = False
         else:
             TOLERANCE = 100