diff --git a/camera_view.py b/camera_view.py
new file mode 100644
index 0000000..1fa50fc
--- /dev/null
+++ b/camera_view.py
@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+
+import os
+import rospy
+from duckietown.dtros import DTROS, NodeType
+from sensor_msgs.msg import CompressedImage
+
+import cv2
+from cv_bridge import CvBridge
+
+class CameraReaderNode(DTROS):
+
+ def __init__(self, node_name):
+ # initialize the DTROS parent class
+ super(CameraReaderNode, self).__init__(node_name=node_name, node_type=NodeType.VISUALIZATION)
+ # static parameters
+ self._vehicle_name = os.environ['VEHICLE_NAME']
+ self._camera_topic = f"/{self._vehicle_name}/camera_node/image/compressed"
+ # bridge between OpenCV and ROS
+ self._bridge = CvBridge()
+ # create window
+ self._window = "camera-reader"
+ cv2.namedWindow(self._window, cv2.WINDOW_AUTOSIZE)
+ # construct subscriber
+ self.sub = rospy.Subscriber(self._camera_topic, CompressedImage, self.callback)
+
+ def callback(self, msg):
+ # convert JPEG bytes to CV image
+ image = self._bridge.compressed_imgmsg_to_cv2(msg)
+ # display frame
+ cv2.imshow(self._window, image)
+ cv2.waitKey(1)
+
+if __name__ == '__main__':
+ # create the node
+ node = CameraReaderNode(node_name='camera_reader_node')
+ # keep spinning
+ rospy.spin()
diff --git a/packages/image_folder_publisher/CMakeLists.txt b/packages/image_folder_publisher/CMakeLists.txt
new file mode 100644
index 0000000..f2a2808
--- /dev/null
+++ b/packages/image_folder_publisher/CMakeLists.txt
@@ -0,0 +1,206 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(image_folder_publisher)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ rospy
+ sensor_msgs
+ std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# sensor_msgs# std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES image_folder_publisher
+# CATKIN_DEPENDS rospy sensor_msgs std_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/image_folder_publisher.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/image_folder_publisher_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_image_folder_publisher.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/packages/image_folder_publisher/CMakeLists.txt:Zone.Identifier b/packages/image_folder_publisher/CMakeLists.txt:Zone.Identifier
new file mode 100644
index 0000000..e69de29
diff --git a/packages/image_folder_publisher/launch/system.launch b/packages/image_folder_publisher/launch/system.launch
new file mode 100644
index 0000000..6a88e8e
--- /dev/null
+++ b/packages/image_folder_publisher/launch/system.launch
@@ -0,0 +1,33 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/packages/image_folder_publisher/launch/system.launch:Zone.Identifier b/packages/image_folder_publisher/launch/system.launch:Zone.Identifier
new file mode 100644
index 0000000..e69de29
diff --git a/packages/image_folder_publisher/package.xml b/packages/image_folder_publisher/package.xml
new file mode 100644
index 0000000..0154eef
--- /dev/null
+++ b/packages/image_folder_publisher/package.xml
@@ -0,0 +1,68 @@
+
+
+ image_folder_publisher
+ 0.0.0
+ The image_folder_publisher package
+
+
+
+
+ user
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+ rospy
+ sensor_msgs
+ std_msgs
+ rospy
+ sensor_msgs
+ std_msgs
+ rospy
+ sensor_msgs
+ std_msgs
+
+
+
+
+
+
+
+
diff --git a/packages/image_folder_publisher/package.xml:Zone.Identifier b/packages/image_folder_publisher/package.xml:Zone.Identifier
new file mode 100644
index 0000000..e69de29
diff --git a/packages/image_folder_publisher/scripts/__pycache__/nir.cpython-38.pyc b/packages/image_folder_publisher/scripts/__pycache__/nir.cpython-38.pyc
new file mode 100644
index 0000000..5c7f56c
Binary files /dev/null and b/packages/image_folder_publisher/scripts/__pycache__/nir.cpython-38.pyc differ
diff --git a/packages/image_folder_publisher/scripts/__pycache__/nir.cpython-38.pyc:Zone.Identifier b/packages/image_folder_publisher/scripts/__pycache__/nir.cpython-38.pyc:Zone.Identifier
new file mode 100644
index 0000000..e69de29
diff --git a/packages/image_folder_publisher/scripts/clear_nir.py b/packages/image_folder_publisher/scripts/clear_nir.py
new file mode 100644
index 0000000..93e3cc2
--- /dev/null
+++ b/packages/image_folder_publisher/scripts/clear_nir.py
@@ -0,0 +1,55 @@
+import cv2
+import numpy as np
+from enum import Enum
+
+class ColorLine(Enum):
+ YELLOW = 1
+ WHITE = 2
+
+
+class FastLaneDetector:
+ def __init__(self, width, height, color: ColorLine):
+ self.width = width
+ self.height = height
+ self.color = color
+
+ # ROI
+ self.y_top = int(height * 0.55)
+ self.y_bottom = int(height * 0.90)
+
+ # HSV masks
+ if color == ColorLine.YELLOW:
+ self.lower = np.array([20, 80, 80])
+ self.upper = np.array([40, 255, 255])
+ else:
+ self.lower = np.array([0, 0, 200])
+ self.upper = np.array([180, 40, 255])
+
+ def process(self, image):
+ hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
+
+ mask = cv2.inRange(hsv, self.lower, self.upper)
+
+ roi = mask[self.y_top:self.y_bottom, :]
+
+ hist = np.sum(roi, axis=0)
+
+ if np.max(hist) < 50:
+ return None, 0.0
+
+ xs = np.arange(self.width)
+ center_x = np.sum(xs * hist) / np.sum(hist)
+
+ robot_center = self.width / 2
+ error = (center_x - robot_center) / robot_center
+
+ return center_x, error
+
+ def draw(self, image, center_x):
+ if center_x is None:
+ return image
+
+ x = int(center_x)
+ cv2.line(image, (x, self.y_top), (x, self.y_bottom), (0,255,0), 2)
+ cv2.rectangle(image, (0, self.y_top), (self.width, self.y_bottom), (255,0,0), 2)
+ return image
\ No newline at end of file
diff --git a/packages/image_folder_publisher/scripts/clear_nir.py:Zone.Identifier b/packages/image_folder_publisher/scripts/clear_nir.py:Zone.Identifier
new file mode 100644
index 0000000..e69de29
diff --git a/packages/image_folder_publisher/scripts/controller_node.py b/packages/image_folder_publisher/scripts/controller_node.py
new file mode 100644
index 0000000..dd71e58
--- /dev/null
+++ b/packages/image_folder_publisher/scripts/controller_node.py
@@ -0,0 +1,49 @@
+#!/usr/bin/env python3
+import rospy
+from geometry_msgs.msg import Twist, Vector3
+from nir import PIDController
+
+
+class ControllerNode:
+ def __init__(self):
+ rospy.init_node("pid_controller")
+
+ self.pid = PIDController(
+ kp_horizontal=0.008,
+ ki_horizontal=0.0001,
+ kd_horizontal=0.0005,
+ kp_angular=0.3,
+ ki_angular=0.0001,
+ kd_angular=0.05,
+ max_linear_velocity=0.25,
+ max_angular_velocity=1.5
+ )
+
+ self.sub = rospy.Subscriber("/lane_state", Vector3, self.callback, queue_size=1)
+ self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
+
+ rospy.loginfo("PID controller started")
+
+ def callback(self, msg):
+ horizontal_error = msg.x
+ angular_error = msg.y
+ valid = msg.z > 0.5
+
+ if not valid:
+ self.publish(0.0, 0.0)
+ return
+
+ v, w = self.pid.update(horizontal_error, angular_error)
+ self.publish(v, w)
+
+ def publish(self, v, w):
+ cmd = Twist()
+ cmd.linear.x = v
+ cmd.angular.z = w
+ #rospy.loginfo(f"PID controller: v={v:.2f}, w={w:.2f}")
+ self.pub.publish(cmd)
+
+
+if __name__ == "__main__":
+ ControllerNode()
+ rospy.spin()
diff --git a/packages/image_folder_publisher/scripts/controller_node.py:Zone.Identifier b/packages/image_folder_publisher/scripts/controller_node.py:Zone.Identifier
new file mode 100644
index 0000000..e69de29
diff --git a/packages/image_folder_publisher/scripts/folder_image_node.py b/packages/image_folder_publisher/scripts/folder_image_node.py
new file mode 100644
index 0000000..75ea4ea
--- /dev/null
+++ b/packages/image_folder_publisher/scripts/folder_image_node.py
@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+import rospy
+from sensor_msgs.msg import Image
+import cv2
+import os
+
+
+class FolderImagePublisher:
+ def __init__(self):
+ rospy.init_node("folder_image_publisher")
+
+ self.folder = rospy.get_param("~folder", "/home/user/images")
+ self.rate = rospy.get_param("~rate", 5)
+
+ self.pub = rospy.Publisher("/image_raw", Image, queue_size=1)
+
+ rospy.loginfo(f"Reading images from: {self.folder}")
+ self.files = sorted([
+ f for f in os.listdir(self.folder)
+ if f.lower().endswith((".jpg", ".png", ".jpeg"))
+ ])
+
+ if not self.files:
+ rospy.logerr("No images found in folder")
+ exit(1)
+
+ self.idx = 0
+ self.run()
+
+ def run(self):
+ r = rospy.Rate(self.rate)
+
+ while not rospy.is_shutdown():
+ path = os.path.join(self.folder, self.files[self.idx])
+ img = cv2.imread(path)
+
+ if img is None:
+ rospy.logwarn(f"Failed to load {path}")
+ continue
+
+ msg = self.cv_to_imgmsg(img)
+ self.pub.publish(msg)
+
+ rospy.loginfo(f"Published {self.files[self.idx]}")
+
+ self.idx = (self.idx + 1) % len(self.files)
+ r.sleep()
+
+ def cv_to_imgmsg(self, img):
+ msg = Image()
+ msg.height, msg.width, _ = img.shape
+ msg.encoding = "bgr8"
+ msg.step = msg.width * 3
+ msg.data = img.tobytes()
+ return msg
+
+
+if __name__ == "__main__":
+ FolderImagePublisher()
diff --git a/packages/image_folder_publisher/scripts/folder_image_node.py:Zone.Identifier b/packages/image_folder_publisher/scripts/folder_image_node.py:Zone.Identifier
new file mode 100644
index 0000000..e69de29
diff --git a/packages/image_folder_publisher/scripts/image_processing_node.py b/packages/image_folder_publisher/scripts/image_processing_node.py
new file mode 100644
index 0000000..a82764b
--- /dev/null
+++ b/packages/image_folder_publisher/scripts/image_processing_node.py
@@ -0,0 +1,179 @@
+#!/usr/bin/env python3
+import rospy
+import cv2
+import numpy as np
+import math
+
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Vector3
+from cv_bridge import CvBridge
+
+from nir import ColorLine, warp_bird_eye
+
+
+class ImageLineProcessingOptimized:
+ def __init__(self, width, height, color: ColorLine):
+ self.width = width
+ self.height = height
+ self.color = color
+
+ if color == ColorLine.yellow:
+ self.lower = np.array([175,165,20], np.uint8)
+ self.upper = np.array([250,250,150], np.uint8)
+ self.left, self.right = 0.0, 0.8
+ else:
+ self.lower = np.array([160,160,160], np.uint8)
+ self.upper = np.array([255,255,255], np.uint8)
+ self.left, self.right = 0.2, 1.0
+
+ self._roi_mask = None
+ self.image = None
+
+ def _get_roi_mask(self):
+ if self._roi_mask is None:
+ mask = np.zeros((self.height, self.width), dtype=np.uint8)
+ vertices = np.array([[
+ (int(self.width * self.left), int(self.height * 0.8)),
+ (int(self.width * self.left), int(self.height * 0.1)),
+ (int(self.width * self.right), int(self.height * 0.1)),
+ (int(self.width * self.right), int(self.height * 0.8)),
+ ]], np.int32)
+ cv2.fillPoly(mask, vertices, 255)
+ self._roi_mask = mask
+ return self._roi_mask
+
+ def region_selection(self, color_mask):
+ return cv2.bitwise_and(color_mask, self._get_roi_mask())
+
+ def detect_line(self, binary):
+ edges = cv2.Canny(binary, 80, 160)
+
+ lines = cv2.HoughLinesP(
+ edges, 1, np.pi/180,
+ threshold=20, minLineLength=30, maxLineGap=10
+ )
+ if lines is None:
+ return None
+
+ pts = []
+ for l in lines:
+ x1,y1,x2,y2 = l[0]
+ if abs(x2-x1) < 5 or abs((y2-y1)/(x2-x1+1e-5)) > 0.3:
+ pts.append((x1,y1))
+ pts.append((x2,y2))
+
+ if len(pts) < 6:
+ return None
+
+ pts = np.array(pts, np.float32)
+ vx, vy, x0, y0 = cv2.fitLine(
+ pts, cv2.DIST_L2, 0, 0.01, 0.01
+ )
+ vx, vy, x0, y0 = vx[0], vy[0], x0[0], y0[0]
+
+ y1 = int(self.height * 0.78)
+ y2 = int(self.height * 0.55)
+ x1 = int((y1 - y0) * vx / vy + x0)
+ x2 = int((y2 - y0) * vx / vy + x0)
+
+ return (x1,y1,x2,y2), float(vx/vy), float(x0 - (vx/vy)*y0)
+
+ def process(self, image):
+ self.image = cv2.blur(image, (3,3))
+
+ mask = cv2.inRange(self.image, self.lower, self.upper)
+ roi = self.region_selection(mask)
+ _, binary = cv2.threshold(roi, 120, 255, cv2.THRESH_BINARY)
+
+ result = self.detect_line(binary)
+ if result is None:
+ return {"valid": False, "line": None, "k": None, "b": None}
+
+ line, k, b = result
+ return {"valid": True, "line": line, "k": k, "b": b}
+
+ def x_at_y(self, model, y):
+ if not model["valid"]:
+ return None
+ return model["k"] * y + model["b"]
+
+ def draw(self, image, model, color=(0,255,0), thickness=2):
+ if not model["valid"]:
+ return image
+ x1,y1,x2,y2 = model["line"]
+ return cv2.line(image, (x1,y1), (x2,y2), color, thickness)
+
+
+class ImageProcessingNode:
+ def __init__(self):
+ rospy.init_node("image_processing_node")
+
+ self.bridge = CvBridge()
+
+ self.sub = rospy.Subscriber("/image_raw", Image, self.callback, queue_size=1)
+ self.pub_debug = rospy.Publisher("/lane_debug", Image, queue_size=1)
+ self.pub_state = rospy.Publisher("/lane_state", Vector3, queue_size=1)
+
+ self.yellow = None
+ self.white = None
+
+ rospy.loginfo("Optimized image processing node started")
+
+ def callback(self, msg):
+ img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+ h, w, _ = img.shape
+
+ bird = warp_bird_eye(img)
+
+ if self.yellow is None:
+ self.yellow = ImageLineProcessingOptimized(w, h, ColorLine.yellow)
+ self.white = ImageLineProcessingOptimized(w, h, ColorLine.white)
+
+ center_x = w / 2
+ bottom_y = int(h * 0.78)
+
+ y_model = self.yellow.process(bird)
+ w_model = self.white.process(bird)
+
+ horizontal_error = 0.0
+ angular_error = 0.0
+ valid = False
+
+ if y_model["valid"] and w_model["valid"]:
+ yx = self.yellow.x_at_y(y_model, bottom_y)
+ wx = self.white.x_at_y(w_model, bottom_y)
+
+ lane_center = (yx + wx) / 2
+ horizontal_error = lane_center - center_x
+
+ ang = (math.atan(y_model["k"]) + math.atan(w_model["k"])) / 2
+ angular_error = ang
+ valid = True
+
+ elif y_model["valid"]:
+ yx = self.yellow.x_at_y(y_model, bottom_y)
+ horizontal_error = yx - (center_x - 100)
+ angular_error = math.atan(y_model["k"])
+ valid = True
+
+ elif w_model["valid"]:
+ wx = self.white.x_at_y(w_model, bottom_y)
+ horizontal_error = wx - (center_x + 100)
+ angular_error = math.atan(w_model["k"])
+ valid = True
+
+ state = Vector3()
+ state.x = horizontal_error
+ state.y = angular_error
+ state.z = 1.0 if valid else 0.0
+ self.pub_state.publish(state)
+
+ dbg = self.yellow.draw(bird.copy(), y_model, (0,255,255))
+ dbg = self.white.draw(dbg, w_model, (255,255,255))
+
+ self.pub_debug.publish(self.bridge.cv2_to_imgmsg(dbg, "bgr8"))
+
+
+if __name__ == "__main__":
+ ImageProcessingNode()
+ rospy.spin()
\ No newline at end of file
diff --git a/packages/image_folder_publisher/scripts/image_processing_node.py:Zone.Identifier b/packages/image_folder_publisher/scripts/image_processing_node.py:Zone.Identifier
new file mode 100644
index 0000000..e69de29
diff --git a/packages/image_folder_publisher/scripts/image_processing_node_old.py b/packages/image_folder_publisher/scripts/image_processing_node_old.py
new file mode 100644
index 0000000..ed9b64b
--- /dev/null
+++ b/packages/image_folder_publisher/scripts/image_processing_node_old.py
@@ -0,0 +1,85 @@
+#!/usr/bin/env python3
+import rospy
+import cv2
+import numpy as np
+import math
+
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Vector3
+from cv_bridge import CvBridge
+
+from nir import ImageLineProcessing, ColorLine, warp_bird_eye
+
+
+class ImageProcessingNode:
+ def __init__(self):
+ rospy.init_node("image_processing_node")
+
+ self.bridge = CvBridge()
+
+ self.sub = rospy.Subscriber("/image_raw", Image, self.callback, queue_size=1)
+ self.pub_debug = rospy.Publisher("/lane_debug", Image, queue_size=1)
+ self.pub_state = rospy.Publisher("/lane_state", Vector3, queue_size=1)
+
+ rospy.loginfo("Image processing node started")
+
+ def callback(self, msg):
+ img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+ h, w, _ = img.shape
+
+ bird = warp_bird_eye(img)
+
+ center_x = w / 2
+ bottom_y = int(h * 0.78)
+
+ yellow = ImageLineProcessing(bird, w, h, ColorLine.yellow)
+ white = ImageLineProcessing(bird, w, h, ColorLine.white)
+
+ y_model = yellow.process()
+ w_model = white.process()
+
+ horizontal_error = 0.0
+ angular_error = 0.0
+ valid = False
+
+ if y_model["valid"] and w_model["valid"]:
+ yx = yellow.x_at_y(y_model, bottom_y)
+ wx = white.x_at_y(w_model, bottom_y)
+
+ lane_center = (yx + wx) / 2
+ horizontal_error = lane_center - center_x
+
+ ang = (math.atan(y_model["k"]) + math.atan(w_model["k"])) / 2
+ angular_error = ang
+
+ valid = True
+
+ elif y_model["valid"]:
+ yx = yellow.x_at_y(y_model, bottom_y)
+ horizontal_error = yx - (center_x - 100)
+ angular_error = math.atan(y_model["k"])
+ valid = True
+
+ elif w_model["valid"]:
+ wx = white.x_at_y(w_model, bottom_y)
+ horizontal_error = wx - (center_x + 100)
+ angular_error = math.atan(w_model["k"])
+ valid = True
+
+ state = Vector3()
+ state.x = horizontal_error
+ state.y = angular_error
+ state.z = 1.0 if valid else 0.0
+ self.pub_state.publish(state)
+
+ # Debug image
+ dbg = yellow.draw(bird.copy(), y_model, (0,255,255))
+ dbg = white.draw(dbg, w_model, (255,255,255))
+
+ dbg_msg = self.bridge.cv2_to_imgmsg(dbg, "bgr8")
+ self.pub_debug.publish(dbg_msg)
+
+
+if __name__ == "__main__":
+ ImageProcessingNode()
+ rospy.spin()
diff --git a/packages/image_folder_publisher/scripts/image_processing_node_old.py:Zone.Identifier b/packages/image_folder_publisher/scripts/image_processing_node_old.py:Zone.Identifier
new file mode 100644
index 0000000..e69de29
diff --git a/packages/image_folder_publisher/scripts/nir.py b/packages/image_folder_publisher/scripts/nir.py
new file mode 100644
index 0000000..80ba25c
--- /dev/null
+++ b/packages/image_folder_publisher/scripts/nir.py
@@ -0,0 +1,378 @@
+import cv2
+import numpy as np
+import enum
+import math
+import os
+import matplotlib.pyplot as plt
+
+@enum.unique
+class ColorLine(enum.Enum):
+ yellow = 2
+ white = 1
+
+
+class ImageLineProcessing:
+ def __init__(self, image, width, height, color: ColorLine):
+ self.image = image #cv2.GaussianBlur(image, (5, 5), 0)
+ self.width = width
+ self.height = height
+ self.color = color
+
+ # Цветовая маска (BGR, Duckietown)
+ if color == ColorLine.yellow:
+ lower = np.array([175, 165, 20], dtype=np.uint8)
+ upper = np.array([250, 250, 150], dtype=np.uint8)
+ else:
+ lower = np.array([160, 160, 160], dtype=np.uint8)
+ upper = np.array([255, 255, 255], dtype=np.uint8)
+
+ self.color_mask = cv2.inRange(self.image, lower, upper)
+
+ # --------------------------------------------------
+ # ROI
+ # --------------------------------------------------
+ def region_selection(self):
+ mask = np.zeros_like(self.color_mask)
+
+ rows, cols = self.color_mask.shape[:2]
+
+ if self.color == ColorLine.yellow:
+ left, right = 0.0, 0.8
+ else:
+ left, right = 0.2, 1.0
+
+ vertices = np.array([[
+ (cols * left, rows * 0.8),
+ (cols * left, rows * 0.1),
+ (cols * right, rows * 0.1),
+ (cols * right, rows * 0.8),
+ ]], dtype=np.int32)
+
+ cv2.fillPoly(mask, vertices, 255)
+ return cv2.bitwise_and(self.color_mask, mask)
+
+ # --------------------------------------------------
+ # Детекция линии (Hough)
+ # --------------------------------------------------
+ def detect_line(self, binary):
+ edges = cv2.Canny(binary, 80, 160)
+
+ params = dict(
+ threshold=20,
+ minLineLength=30,
+ maxLineGap=10
+ )
+
+ lines = cv2.HoughLinesP(edges, 1, np.pi / 180, **params)
+ if lines is None:
+ return None
+
+ # Фильтрация почти горизонтальных линий
+ candidates = []
+ for l in lines:
+ x1, y1, x2, y2 = l[0]
+ if abs(x2 - x1) < 5: # почти вертикальная — ок
+ candidates.append((x1, y1, x2, y2))
+ else:
+ slope = abs((y2 - y1) / (x2 - x1))
+ if slope > 0.3:
+ candidates.append((x1, y1, x2, y2))
+
+ if not candidates:
+ return None
+
+ # Усреднение по всем сегментам
+ xs, ys = [], []
+ for x1, y1, x2, y2 in candidates:
+ xs += [x1, x2]
+ ys += [y1, y2]
+
+ if len(xs) < 2:
+ return None
+
+ # Линейная регрессия x(y)
+ fit = np.polyfit(ys, xs, 1)
+ k, b = fit
+
+ y_bottom = int(self.height * 0.78)
+ y_top = int(self.height * 0.55)
+
+ x_bottom = int(k * y_bottom + b)
+ x_top = int(k * y_top + b)
+
+ return (x_bottom, y_bottom, x_top, y_top), k, b
+
+ # --------------------------------------------------
+ # Основной метод
+ # --------------------------------------------------
+ def process(self):
+ roi = self.region_selection()
+ _, binary = cv2.threshold(roi, 120, 255, cv2.THRESH_BINARY)
+
+ result = self.detect_line(binary)
+ if result is None:
+ return {
+ "valid": False,
+ "line": None,
+ "k": None,
+ "b": None
+ }
+
+ line, k, b = result
+ return {
+ "valid": True,
+ "line": line,
+ "k": k,
+ "b": b
+ }
+
+ def x_at_y(self, model, y):
+ if not model["valid"]:
+ return None
+ return model["k"] * y + model["b"]
+
+ def draw(self, image, model, color=(0, 255, 0), thickness=2):
+ if not model["valid"]:
+ return image
+ x1, y1, x2, y2 = model["line"]
+ return cv2.line(image.copy(), (x1, y1), (x2, y2), color, thickness)
+
+
+def get_bird_eye_transform():
+ CAMERA_FOV_Y = 75.0 # градусы
+ CAMERA_ANGLE = 19.15 * math.pi / 180.0 # радианы
+ CAMERA_FLOOR_DIST = 0.108 # м
+
+ DEFAULT_CAMERA_WIDTH = 640
+ DEFAULT_CAMERA_HEIGHT = 480
+
+ w = DEFAULT_CAMERA_WIDTH
+ h = DEFAULT_CAMERA_HEIGHT
+
+ # Вертикальное и горизонтальное поле зрения
+ fov_y = math.radians(CAMERA_FOV_Y)
+ fov_x = 2 * math.atan(math.tan(fov_y / 2) * (w / h))
+
+ # Фокальные расстояния в пикселях
+ fx = w / (2 * math.tan(fov_x / 2))
+ fy = h / (2 * math.tan(fov_y / 2))
+
+ # Центр проекции
+ cx, cy = w / 2, h / 2
+
+ # 4 точки в нижней части изображения (трапеция дороги)
+ img_pts = np.float32([
+ [w*0.2, h*0.9], # левый низ
+ [w*0.8, h*0.9], # правый низ
+ [w*0.45, h*0.6], # левый верх
+ [w*0.55, h*0.6] # правый верх
+ ])
+
+ def img_to_ground(u, v):
+ # нормализованные координаты
+ x_cam = (u - cx) / fx
+ y_cam = (v - cy) / fy
+
+ # направляющий вектор луча в системе камеры
+ ray = np.array([x_cam, 1.0, y_cam]) # (x, forward, y)
+
+ # поворот камеры вниз
+ R = np.array([
+ [1, 0, 0],
+ [0, math.cos(-CAMERA_ANGLE), -math.sin(-CAMERA_ANGLE)],
+ [0, math.sin(-CAMERA_ANGLE), math.cos(-CAMERA_ANGLE)]
+ ])
+ ray = R @ ray
+
+ # пересечение с плоскостью z=0
+ t = CAMERA_FLOOR_DIST / (-ray[2])
+ X = ray[0]*t
+ Y = ray[1]*t
+ return [X, Y]
+
+ world_pts = np.float32([img_to_ground(u, v) for (u, v) in img_pts])
+
+ minx, miny = np.min(world_pts, axis=0)
+ maxx, maxy = np.max(world_pts, axis=0)
+
+ world_norm = (world_pts - [minx, miny]) / ([maxx-minx, maxy-miny])
+ world_norm[:,0] *= w
+ world_norm[:,1] *= h
+
+ M = cv2.getPerspectiveTransform(img_pts, world_norm)
+ return M
+
+DEFAULT_CAMERA_WIDTH = 640
+DEFAULT_CAMERA_HEIGHT = 480
+PI = 3.1415926
+
+alpha = (13 - 90) * PI / 180
+beta = (89 - 90) * PI / 180
+gamma = (95 - 90) * PI / 180
+focalLength = 653 #cv2.getTrackbarPos("f", "Result")
+dist = 666 #cv2.getTrackbarPos("Distance", "Result")
+
+image_size = (DEFAULT_CAMERA_WIDTH, DEFAULT_CAMERA_HEIGHT)
+w, h = image_size
+
+A1 = np.array([[1, 0, -w / 2],
+ [0, 1, -h / 2],
+ [0, 0, 0],
+ [0, 0, 1]], dtype=np.float32)
+
+RX = np.array([[1, 0, 0, 0],
+ [0, math.cos(alpha), -math.sin(alpha), 0],
+ [0, math.sin(alpha), math.cos(alpha), 0],
+ [0, 0, 0, 1]], dtype=np.float32)
+
+RY = np.array([[math.cos(beta), 0, -math.sin(beta), 0],
+ [0, 1, 0, 0],
+ [math.sin(beta), 0, math.cos(beta), 0],
+ [0, 0, 0, 1]], dtype=np.float32)
+
+RZ = np.array([[math.cos(gamma), -math.sin(gamma), 0, 0],
+ [math.sin(gamma), math.cos(gamma), 0, 0],
+ [0, 0, 1, 0],
+ [0, 0, 0, 1]], dtype=np.float32)
+
+R = np.dot(np.dot(RX, RY), RZ)
+
+T = np.array([[1, 0, 0, 0],
+ [0, 1, 0, 0],
+ [0, 0, 1, dist],
+ [0, 0, 0, 1]], dtype=np.float32)
+
+K = np.array([[focalLength, 0, w / 2, 0],
+ [0, focalLength, h / 2, 0],
+ [0, 0, 1, 0]], dtype=np.float32)
+
+transformationMat = np.dot(np.dot(np.dot(K, T), R), A1)
+
+def warp_bird_eye(image):
+ destination = cv2.warpPerspective(image, transformationMat, image_size, flags=cv2.INTER_CUBIC + cv2.WARP_INVERSE_MAP)
+ return destination
+
+import time
+import math
+
+class PIDController:
+ def __init__(self,
+ kp_horizontal, ki_horizontal, kd_horizontal,
+ kp_angular, ki_angular, kd_angular,
+ max_linear_velocity=0.5,
+ max_angular_velocity=2.0):
+ """Initializes the PID controller.
+
+ Args:
+ kp_horizontal (float): Proportional gain for horizontal error.
+ ki_horizontal (float): Integral gain for horizontal error.
+ kd_horizontal (float): Derivative gain for horizontal error.
+ kp_angular (float): Proportional gain for angular error.
+ ki_angular (float): Integral gain for angular error.
+ kd_angular (float): Derivative gain for angular error.
+ max_linear_velocity (float): Maximum allowed linear velocity (e.g., m/s).
+ max_angular_velocity (float): Maximum allowed angular velocity (e.g., rad/s).
+ """
+ self.kp_h = kp_horizontal
+ self.ki_h = ki_horizontal
+ self.kd_h = kd_horizontal
+
+ self.kp_a = kp_angular
+ self.ki_a = ki_angular
+ self.kd_a = kd_angular
+
+ self.max_linear_v = max_linear_velocity
+ self.max_angular_v = max_angular_velocity
+
+ # Internal state for horizontal error PID
+ self.prev_error_h = 0.0
+ self.integral_h = 0.0
+
+ # Internal state for angular error PID
+ self.prev_error_a = 0.0
+ self.integral_a = 0.0
+
+ self.last_time = None
+
+ def update(self, horizontal_error, angular_error):
+ """Updates the PID controller with new errors and computes control outputs.
+
+ Args:
+ horizontal_error (float): The horizontal deviation from the desired path (e.g., in pixels).
+ Positive if lane center is to the right of the robot's center.
+ angular_error (float): The angular deviation from the desired heading (e.g., in radians).
+ Positive if the lane is slanting to the right relative to robot's heading.
+
+ Returns:
+ tuple: (linear_velocity, angular_velocity) in the robot's units.
+ """
+ current_time = time.time()
+ if self.last_time is None:
+ # First call, initialize and return base velocities
+ self.last_time = current_time
+ self.prev_error_h = horizontal_error
+ self.prev_error_a = angular_error
+ return self.max_linear_v, 0.0
+
+ dt = current_time - self.last_time
+ if dt <= 0:
+ # Avoid division by zero or negative time step
+ return self.max_linear_v, 0.0
+
+ # PID for Horizontal Error (influences angular velocity)
+ self.integral_h += horizontal_error * dt
+ derivative_h = (horizontal_error - self.prev_error_h) / dt
+ output_h = self.kp_h * horizontal_error + self.ki_h * self.integral_h + self.kd_h * derivative_h
+ self.prev_error_h = horizontal_error
+
+ # PID for Angular Error (influences angular velocity)
+ self.integral_a += angular_error * dt
+ derivative_a = (angular_error - self.prev_error_a) / dt
+ output_a = self.kp_a * angular_error + self.ki_a * self.integral_a + self.kd_a * derivative_a
+ self.prev_error_a = angular_error
+
+ # Combine outputs to determine angular velocity
+ # Convention: Positive angular_velocity for left turn, negative for right turn.
+ # - A positive horizontal_error (lane right) requires a right turn (negative omega).
+ # - A positive angular_error (lane slanting right) requires a right turn (negative omega).
+ angular_velocity = -output_h - output_a
+
+ # Constrain angular velocity to defined limits
+ angular_velocity = max(-self.max_angular_v, min(self.max_angular_v, angular_velocity))
+
+ # Linear velocity (can be constant or reduced based on angular_velocity)
+ linear_velocity = self.max_linear_v
+ # Optional: reduce linear velocity for sharp turns
+ # linear_velocity = self.max_linear_v * (1 - abs(angular_velocity) / (2 * self.max_angular_v))
+ # linear_velocity = max(0.0, min(self.max_linear_v, linear_velocity))
+
+ self.last_time = current_time
+ return linear_velocity, angular_velocity
+
+P = 1/30
+I = 1/20
+D = 1/20
+pid_controller_old = PIDController(
+ kp_horizontal=0.02, ki_horizontal=0.0, kd_horizontal=0.001, # Example gains for horizontal control
+ kp_angular=0.5, ki_angular=0.0, kd_angular=0.1, # Example gains for angular control
+ max_linear_velocity=0.3, # Max linear velocity in m/s (adjust for Duckietown)
+ max_angular_velocity=2.0 # Max angular velocity in rad/s (adjust for Duckietown)
+)
+
+pid_controller = PIDController(
+ kp_horizontal=0.008, # Уменьшил пропорциональную составляющую
+ ki_horizontal=0.0001, # Добавил небольшую интегральную
+ kd_horizontal=0.0005, # Уменьшил дифференциальную
+
+ kp_angular=0.3, # Уменьшил для более плавного управления
+ ki_angular=0.0001, # Добавил интегральную для устранения статической ошибки
+ kd_angular=0.05, # Уменьшил дифференциальную
+
+ max_linear_velocity=0.25, # Немного уменьшил скорость
+ max_angular_velocity=1.5 # Уменьшил максимальную угловую скорость
+)
+horizontal_error = 0.0
+angular_error = 0.0
+linear_velocity = 0.0
+angular_velocity = 0.0
+
diff --git a/packages/image_folder_publisher/scripts/nir.py:Zone.Identifier b/packages/image_folder_publisher/scripts/nir.py:Zone.Identifier
new file mode 100644
index 0000000..e69de29
diff --git a/test.png b/test.png
new file mode 100644
index 0000000..9089c60
Binary files /dev/null and b/test.png differ