From 38ca81f51aecfe728da072bf03e4bcab357a193a Mon Sep 17 00:00:00 2001
From: Hirthik Gopal Shanmugam <141051449+hgs2007@users.noreply.github.com>
Date: Tue, 30 Sep 2025 20:00:34 -0400
Subject: [PATCH 01/21] black and white video
testing
---
opencv_testing/opencv_testing.py | 19 +++++++++++++++++++
1 file changed, 19 insertions(+)
create mode 100644 opencv_testing/opencv_testing.py
diff --git a/opencv_testing/opencv_testing.py b/opencv_testing/opencv_testing.py
new file mode 100644
index 00000000..8edac464
--- /dev/null
+++ b/opencv_testing/opencv_testing.py
@@ -0,0 +1,19 @@
+import cv2
+
+cap = cv2.VideoCapture("IMG_8824.mp4") # or 0 for webcam
+
+while True:
+ ret, frame = cap.read()
+ if not ret:
+ break # end of video
+
+ gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+
+ cv2.imshow("Video", gray_frame) # show the video in a window
+
+ # Press 'q' to exit early
+ if cv2.waitKey(25) & 0xFF == ord('q'):
+ break
+
+cap.release()
+cv2.destroyAllWindows()
From af77f62ac2c1a3968cb177b1a5688474369e68ff Mon Sep 17 00:00:00 2001
From: Hirthik Gopal Shanmugam <141051449+hgs2007@users.noreply.github.com>
Date: Tue, 21 Oct 2025 18:24:01 -0400
Subject: [PATCH 02/21] calculates speed, steering input, and acceleration
using angles
---
opencv_testing/opencv_testing.py | 6 ++--
opencv_testing/pathfinder.py | 50 ++++++++++++++++++++++++++++++++
2 files changed, 53 insertions(+), 3 deletions(-)
create mode 100644 opencv_testing/pathfinder.py
diff --git a/opencv_testing/opencv_testing.py b/opencv_testing/opencv_testing.py
index 8edac464..dc56c1f4 100644
--- a/opencv_testing/opencv_testing.py
+++ b/opencv_testing/opencv_testing.py
@@ -1,15 +1,15 @@
import cv2
-cap = cv2.VideoCapture("IMG_8824.mp4") # or 0 for webcam
+cap = cv2.VideoCapture("IMG_8824.mp4")
while True:
ret, frame = cap.read()
if not ret:
- break # end of video
+ break
gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
- cv2.imshow("Video", gray_frame) # show the video in a window
+ cv2.imshow("Video", gray_frame) #shows the video
# Press 'q' to exit early
if cv2.waitKey(25) & 0xFF == ord('q'):
diff --git a/opencv_testing/pathfinder.py b/opencv_testing/pathfinder.py
new file mode 100644
index 00000000..22a52d5e
--- /dev/null
+++ b/opencv_testing/pathfinder.py
@@ -0,0 +1,50 @@
+import time
+import os
+
+speed = 0.0 # mph
+max_accel = 3.0 # mph per second
+max_steering = 25 # degrees
+time_step = 1 # seconds
+
+theta1 = -5
+theta2 = 5
+
+for t in range(1, 31):
+ if t <= 10:
+ theta1 = -5
+ theta2 = 5
+ elif 10 < t <= 20:
+ theta1 -= 2
+ theta2 += 0.5
+ elif 20 < t <= 30:
+ theta1 += 2
+ theta2 += 2
+
+ desired_heading = (theta1 + theta2) / 2 #average of both angles
+
+ if abs(desired_heading) < 10:
+ target_speed = 30 #max speed on straights
+ else:
+ target_speed = 15 #target speed on turns
+
+ if speed < target_speed:
+ speed += max_accel * time_step #physics c: mechanics
+ if speed > target_speed:
+ speed = target_speed
+ else:
+ speed -= max_accel * time_step
+ if speed < target_speed:
+ speed = target_speed
+
+ steering_command = max(min(desired_heading, max_steering), -max_steering)
+
+ os.system('cls' if os.name == 'nt' else 'clear')
+
+ print(f"Time: {t} sec")
+ print(f"Theta1: {theta1:.1f}°, Theta2: {theta2:.1f}°")
+ print(f"Desired Heading: {desired_heading:.1f}°")
+ print(f"Target Speed: {target_speed} mph | Current Speed: {speed:.1f} mph")
+ print(f"Steering Command: {steering_command:.1f}° {'Left' if steering_command < 0 else 'Right' if steering_command > 0 else 'Straight'}")
+
+ time.sleep(1)
+
From 74b44e66ef6a8fe5eb81ba3bc3449a6c8c3d4681 Mon Sep 17 00:00:00 2001
From: Hirthik Gopal Shanmugam <141051449+hgs2007@users.noreply.github.com>
Date: Tue, 21 Oct 2025 18:32:54 -0400
Subject: [PATCH 03/21] calculates angles
---
opencv_testing/angle_calculator.py | 184 +++++++++++++++++++++++++++++
1 file changed, 184 insertions(+)
create mode 100644 opencv_testing/angle_calculator.py
diff --git a/opencv_testing/angle_calculator.py b/opencv_testing/angle_calculator.py
new file mode 100644
index 00000000..54488c3a
--- /dev/null
+++ b/opencv_testing/angle_calculator.py
@@ -0,0 +1,184 @@
+"""
+Compute theta_left and theta_right from a video.
+Requirements: opencv-python (cv2), numpy
+Usage: tweak CAMERA_FX or HFOV_HORIZONTAL to match your camera.
+"""
+
+import cv2
+import numpy as np
+import math
+
+# ----------------- CONFIG -----------------
+VIDEO_PATH = "IMG_8824.mp4" # change to your file or use 0 for webcam
+USE_CALIBRATION = False # True if you have fx, cx from calibration
+CAMERA_FX = 800.0 # focal length in pixels (only if USE_CALIBRATION)
+CAMERA_CX = None # principal point x; if None -> image_width/2
+HFOV_DEG = 70.0 # horizontal field of view (deg) if no fx available
+
+SMOOTH_ALPHA = 0.7 # for low-pass filtering of theta
+CANNY_THRESH1 = 50
+CANNY_THRESH2 = 150
+
+# Ray sampling parameters
+MAX_SAMPLE_DIST = 400 # max pixels to scan along each ray
+FORWARD_STEP = 1 # pixels per sample along forward ray
+RIGHT_STEP = 1 # pixels per sample along right ray
+
+# -----------------------------------------
+
+def compute_fx_from_hfov(width, hfov_deg):
+ hfov = math.radians(hfov_deg)
+ return (width / 2.0) / math.tan(hfov / 2.0)
+
+def pixel_to_angle(u, fx, cx):
+ # returns angle in degrees, negative = left of center
+ return math.degrees(math.atan2((u - cx), fx))
+
+def find_intersection_along_column(mask, col, start_row, step=1, max_dist=400):
+ """
+ Scan downwards along column 'col' starting at start_row (row index),
+ return (u,v) of first mask nonzero pixel, or None if none found.
+ """
+ h, w = mask.shape
+ row = start_row
+ dist = 0
+ while dist < max_dist and 0 <= row < h:
+ if mask[row, col]:
+ return col, row
+ row += step
+ dist += abs(step)
+ return None
+
+def find_intersection_along_row(mask, row, start_col, step=1, max_dist=400):
+ """
+ Scan rightwards along row 'row' starting at start_col, return first mask hit.
+ """
+ h, w = mask.shape
+ col = start_col
+ dist = 0
+ while dist < max_dist and 0 <= col < w:
+ if mask[row, col]:
+ return col, row
+ col += step
+ dist += abs(step)
+ return None
+
+def main():
+ cap = cv2.VideoCapture(VIDEO_PATH)
+ if not cap.isOpened():
+ print("Cannot open video:", VIDEO_PATH)
+ return
+
+ theta_left_f = None
+ theta_right_f = None
+
+ while True:
+ ret, frame = cap.read()
+ if not ret:
+ break
+
+ h, w = frame.shape[:2]
+ cx = CAMERA_CX if CAMERA_CX is not None else w / 2.0
+ if USE_CALIBRATION:
+ fx = CAMERA_FX
+ else:
+ fx = compute_fx_from_hfov(w, HFOV_DEG)
+
+ # 1) preprocess and boundary mask
+ gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+ blur = cv2.GaussianBlur(gray, (5,5), 0)
+ edges = cv2.Canny(blur, CANNY_THRESH1, CANNY_THRESH2)
+ # optional morphology to fill gaps
+ kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5,5))
+ mask = cv2.morphologyEx(edges, cv2.MORPH_CLOSE, kernel)
+
+ # 2) define rays: use image center
+ center_col = int(round(cx))
+ center_row = int(round(h / 2))
+
+ # forward ray: downwards from center_row toward bottom (increasing row)
+ f_hit = find_intersection_along_column(mask, center_col, center_row, step=FORWARD_STEP, max_dist=MAX_SAMPLE_DIST)
+
+ # right ray: from center out to the right along center_row
+ r_hit = find_intersection_along_row(mask, center_row, center_col, step=RIGHT_STEP, max_dist=MAX_SAMPLE_DIST)
+
+ # convert hits to angles (default to None/confidence if not found)
+ theta_left = None
+ theta_right = None
+
+ if f_hit is not None:
+ u_f, v_f = f_hit
+ theta_forward = pixel_to_angle(u_f, fx, cx) # bearing of forward intersection
+ # If the forward ray hits boundary on right side of center, we can treat that as right boundary?
+ # However your definition: theta_left is angle from left boundary to center — we approximate:
+ # We'll set theta_left to the bearing of the left-side boundary detected forward of center,
+ # but since forward ray is center column, use it as a nearby boundary reading if needed.
+ # For consistent approach: treat forward ray hit on its x location to compute whichever boundary that represents.
+ if u_f < cx:
+ theta_left = pixel_to_angle(u_f, fx, cx)
+ else:
+ theta_right = pixel_to_angle(u_f, fx, cx)
+
+ if r_hit is not None:
+ u_r, v_r = r_hit
+ # point to the right of center -> this is likely the right boundary
+ theta_right = pixel_to_angle(u_r, fx, cx)
+
+ # If we failed to find one of them from the rays, try alternate strategy:
+ # find contours and pick nearest contour point to the ray direction (omitted for brevity),
+ # or fallback to last frame value.
+
+ # fallback: if missing, reuse previous smoothed value or compute from any contour
+ if theta_left is None and theta_left_f is not None:
+ theta_left = theta_left_f
+ if theta_right is None and theta_right_f is not None:
+ theta_right = theta_right_f
+
+ # smoothing
+ if theta_left is not None:
+ theta_left_f = theta_left if theta_left_f is None else (SMOOTH_ALPHA * theta_left_f + (1-SMOOTH_ALPHA)*theta_left)
+ if theta_right is not None:
+ theta_right_f = theta_right if theta_right_f is None else (SMOOTH_ALPHA * theta_right_f + (1-SMOOTH_ALPHA)*theta_right)
+
+ # compute desired heading if both exist (or using whichever exists)
+ desired_heading = None
+ if (theta_left_f is not None) and (theta_right_f is not None):
+ desired_heading = 0.5 * (theta_left_f + theta_right_f)
+ elif theta_left_f is not None:
+ desired_heading = theta_left_f # single-side fallback
+ elif theta_right_f is not None:
+ desired_heading = theta_right_f
+
+ # Display overlay for debugging
+ vis = frame.copy()
+ # draw rays
+ cv2.line(vis, (center_col, center_row), (center_col, min(h, center_row + MAX_SAMPLE_DIST)), (0,255,0), 1)
+ cv2.line(vis, (center_col, center_row), (min(w-1, center_col + MAX_SAMPLE_DIST), center_row), (0,255,0), 1)
+ if f_hit is not None:
+ cv2.circle(vis, (f_hit[0], f_hit[1]), 6, (0,0,255), -1)
+ if r_hit is not None:
+ cv2.circle(vis, (r_hit[0], r_hit[1]), 6, (255,0,0), -1)
+
+ # text
+ info = [
+ f"theta_left_f: {theta_left_f:.2f}" if theta_left_f is not None else "theta_left_f: N/A",
+ f"theta_right_f: {theta_right_f:.2f}" if theta_right_f is not None else "theta_right_f: N/A",
+ f"desired_heading: {desired_heading:.2f}" if desired_heading is not None else "desired_heading: N/A"
+ ]
+ y = 30
+ for line in info:
+ cv2.putText(vis, line, (10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,255), 2)
+ y += 25
+
+ cv2.imshow("vis", vis)
+ cv2.imshow("mask", mask)
+
+ # press q to quit
+ if cv2.waitKey(1) & 0xFF == ord('q'):
+ break
+
+ cap.release()
+ cv2.destroyAllWindows()
+
+if __name__ == "__main__":
+ main()
From cc8521381e19a5ea2a322a2d4614fe60d63383f0 Mon Sep 17 00:00:00 2001
From: Hirthik Gopal Shanmugam <141051449+hgs2007@users.noreply.github.com>
Date: Sun, 2 Nov 2025 16:57:07 -0500
Subject: [PATCH 04/21] pathfinder file
---
.github/workflows/build-devcontainer.yml | 1 -
.gitignore | 12 +-
compose/docker-compose.yml | 2 +-
install/.colcon_install_layout | 1 +
install/COLCON_IGNORE | 0
install/_local_setup_util_ps1.py | 407 ++++++++++++++++++
install/_local_setup_util_sh.py | 407 ++++++++++++++++++
.../resource_index/packages/autonomous_kart | 0
.../hook/ament_prefix_path.dsv | 1 +
.../hook/ament_prefix_path.ps1 | 3 +
.../autonomous_kart/hook/ament_prefix_path.sh | 3 +
.../share/autonomous_kart/hook/pythonpath.dsv | 1 +
.../share/autonomous_kart/hook/pythonpath.ps1 | 3 +
.../share/autonomous_kart/hook/pythonpath.sh | 3 +
.../launch/bringup_pi.launch.py | 49 +++
.../launch/bringup_sim.launch.py | 45 ++
.../share/autonomous_kart/package.bash | 31 ++
.../share/autonomous_kart/package.dsv | 6 +
.../share/autonomous_kart/package.ps1 | 116 +++++
.../share/autonomous_kart/package.sh | 87 ++++
.../share/autonomous_kart/package.xml | 23 +
.../share/autonomous_kart/package.zsh | 42 ++
.../share/autonomous_kart/params/camera.yaml | 5 +
.../autonomous_kart/params/controller.yaml | 8 +
.../share/autonomous_kart/params/gps.yaml | 0
.../share/autonomous_kart/params/planner.yaml | 1 +
.../share/autonomous_kart/params/safety.yaml | 1 +
.../colcon-core/packages/autonomous_kart | 1 +
install/local_setup.bash | 121 ++++++
install/local_setup.ps1 | 55 +++
install/local_setup.sh | 137 ++++++
install/local_setup.zsh | 134 ++++++
install/setup.bash | 31 ++
install/setup.ps1 | 29 ++
install/setup.sh | 45 ++
install/setup.zsh | 31 ++
log/COLCON_IGNORE | 0
log/latest | 1 +
log/latest_build | 1 +
opencv_testing/pathfinder.py | 2 +-
requirements.txt | 2 +-
scripts/devcontainer_post_create.sh | 3 +
scripts/run_sim.sh | 3 +
scripts/start.bash | 8 +
.../launch/bringup_sim.launch.py | 17 +-
.../nodes/camera/camera_node.py | 12 +-
.../autonomous_kart/nodes/motor/motor_node.py | 4 +-
.../opencv_pathfinder_node.py | 18 +-
.../nodes/pathfinder/pathfinder.py | 42 +-
.../nodes/pathfinder/pathfinder_node.py | 34 +-
.../nodes/steering/steering_node.py | 4 +-
.../autonomous_kart/params/camera.yaml | 2 +-
.../autonomous_kart/params/system.yaml | 4 +
53 files changed, 1943 insertions(+), 56 deletions(-)
create mode 100644 install/.colcon_install_layout
create mode 100644 install/COLCON_IGNORE
create mode 100644 install/_local_setup_util_ps1.py
create mode 100644 install/_local_setup_util_sh.py
create mode 100644 install/autonomous_kart/share/ament_index/resource_index/packages/autonomous_kart
create mode 100644 install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.dsv
create mode 100644 install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.ps1
create mode 100644 install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.sh
create mode 100644 install/autonomous_kart/share/autonomous_kart/hook/pythonpath.dsv
create mode 100644 install/autonomous_kart/share/autonomous_kart/hook/pythonpath.ps1
create mode 100644 install/autonomous_kart/share/autonomous_kart/hook/pythonpath.sh
create mode 100644 install/autonomous_kart/share/autonomous_kart/launch/bringup_pi.launch.py
create mode 100644 install/autonomous_kart/share/autonomous_kart/launch/bringup_sim.launch.py
create mode 100644 install/autonomous_kart/share/autonomous_kart/package.bash
create mode 100644 install/autonomous_kart/share/autonomous_kart/package.dsv
create mode 100644 install/autonomous_kart/share/autonomous_kart/package.ps1
create mode 100644 install/autonomous_kart/share/autonomous_kart/package.sh
create mode 100644 install/autonomous_kart/share/autonomous_kart/package.xml
create mode 100644 install/autonomous_kart/share/autonomous_kart/package.zsh
create mode 100644 install/autonomous_kart/share/autonomous_kart/params/camera.yaml
create mode 100644 install/autonomous_kart/share/autonomous_kart/params/controller.yaml
create mode 100644 install/autonomous_kart/share/autonomous_kart/params/gps.yaml
create mode 100644 install/autonomous_kart/share/autonomous_kart/params/planner.yaml
create mode 100644 install/autonomous_kart/share/autonomous_kart/params/safety.yaml
create mode 100644 install/autonomous_kart/share/colcon-core/packages/autonomous_kart
create mode 100644 install/local_setup.bash
create mode 100644 install/local_setup.ps1
create mode 100644 install/local_setup.sh
create mode 100644 install/local_setup.zsh
create mode 100644 install/setup.bash
create mode 100644 install/setup.ps1
create mode 100644 install/setup.sh
create mode 100644 install/setup.zsh
create mode 100644 log/COLCON_IGNORE
create mode 120000 log/latest
create mode 120000 log/latest_build
create mode 100644 scripts/run_sim.sh
create mode 100644 scripts/start.bash
create mode 100644 src/autonomous_kart/autonomous_kart/params/system.yaml
diff --git a/.github/workflows/build-devcontainer.yml b/.github/workflows/build-devcontainer.yml
index 43141fe9..d99b4563 100644
--- a/.github/workflows/build-devcontainer.yml
+++ b/.github/workflows/build-devcontainer.yml
@@ -9,7 +9,6 @@ on:
- "compose/**"
- "requirements.txt"
- ".devcontainer/**"
- - "scripts/**"
- ".github/workflows/build-devcontainer.yml"
workflow_dispatch:
diff --git a/.gitignore b/.gitignore
index 7dbdfee6..7bfe79ad 100644
--- a/.gitignore
+++ b/.gitignore
@@ -168,4 +168,14 @@ cython_debug/
#.idea/
# PyPI configuration file
-.pypirc
\ No newline at end of file
+.pypirc
+
+# Too large file size
+.vscode/browse.vc.db
+
+# Ros build ignored
+install
+log
+build
+opencv_testing/IMG_8824.MP4
+opencv_testing/IMG_8824
\ No newline at end of file
diff --git a/compose/docker-compose.yml b/compose/docker-compose.yml
index 8fd24078..d53dbe7b 100644
--- a/compose/docker-compose.yml
+++ b/compose/docker-compose.yml
@@ -1,7 +1,7 @@
version: "3.8"
services:
dev:
- image: ghcr.io/evc-purdue/ros2-humble-dev:latest
+ image: ghcr.io/evc-purdue/ros2-humble-dev:main
container_name: ros2-dev
hostname: ros2-dev
volumes:
diff --git a/install/.colcon_install_layout b/install/.colcon_install_layout
new file mode 100644
index 00000000..3aad5336
--- /dev/null
+++ b/install/.colcon_install_layout
@@ -0,0 +1 @@
+isolated
diff --git a/install/COLCON_IGNORE b/install/COLCON_IGNORE
new file mode 100644
index 00000000..e69de29b
diff --git a/install/_local_setup_util_ps1.py b/install/_local_setup_util_ps1.py
new file mode 100644
index 00000000..3c6d9e87
--- /dev/null
+++ b/install/_local_setup_util_ps1.py
@@ -0,0 +1,407 @@
+# Copyright 2016-2019 Dirk Thomas
+# Licensed under the Apache License, Version 2.0
+
+import argparse
+from collections import OrderedDict
+import os
+from pathlib import Path
+import sys
+
+
+FORMAT_STR_COMMENT_LINE = '# {comment}'
+FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"'
+FORMAT_STR_USE_ENV_VAR = '$env:{name}'
+FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' # noqa: E501
+FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' # noqa: E501
+FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' # noqa: E501
+
+DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate'
+DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate'
+DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists'
+DSV_TYPE_SET = 'set'
+DSV_TYPE_SET_IF_UNSET = 'set-if-unset'
+DSV_TYPE_SOURCE = 'source'
+
+
+def main(argv=sys.argv[1:]): # noqa: D103
+ parser = argparse.ArgumentParser(
+ description='Output shell commands for the packages in topological '
+ 'order')
+ parser.add_argument(
+ 'primary_extension',
+ help='The file extension of the primary shell')
+ parser.add_argument(
+ 'additional_extension', nargs='?',
+ help='The additional file extension to be considered')
+ parser.add_argument(
+ '--merged-install', action='store_true',
+ help='All install prefixes are merged into a single location')
+ args = parser.parse_args(argv)
+
+ packages = get_packages(Path(__file__).parent, args.merged_install)
+
+ ordered_packages = order_packages(packages)
+ for pkg_name in ordered_packages:
+ if _include_comments():
+ print(
+ FORMAT_STR_COMMENT_LINE.format_map(
+ {'comment': 'Package: ' + pkg_name}))
+ prefix = os.path.abspath(os.path.dirname(__file__))
+ if not args.merged_install:
+ prefix = os.path.join(prefix, pkg_name)
+ for line in get_commands(
+ pkg_name, prefix, args.primary_extension,
+ args.additional_extension
+ ):
+ print(line)
+
+ for line in _remove_ending_separators():
+ print(line)
+
+
+def get_packages(prefix_path, merged_install):
+ """
+ Find packages based on colcon-specific files created during installation.
+
+ :param Path prefix_path: The install prefix path of all packages
+ :param bool merged_install: The flag if the packages are all installed
+ directly in the prefix or if each package is installed in a subdirectory
+ named after the package
+ :returns: A mapping from the package name to the set of runtime
+ dependencies
+ :rtype: dict
+ """
+ packages = {}
+ # since importing colcon_core isn't feasible here the following constant
+ # must match colcon_core.location.get_relative_package_index_path()
+ subdirectory = 'share/colcon-core/packages'
+ if merged_install:
+ # return if workspace is empty
+ if not (prefix_path / subdirectory).is_dir():
+ return packages
+ # find all files in the subdirectory
+ for p in (prefix_path / subdirectory).iterdir():
+ if not p.is_file():
+ continue
+ if p.name.startswith('.'):
+ continue
+ add_package_runtime_dependencies(p, packages)
+ else:
+ # for each subdirectory look for the package specific file
+ for p in prefix_path.iterdir():
+ if not p.is_dir():
+ continue
+ if p.name.startswith('.'):
+ continue
+ p = p / subdirectory / p.name
+ if p.is_file():
+ add_package_runtime_dependencies(p, packages)
+
+ # remove unknown dependencies
+ pkg_names = set(packages.keys())
+ for k in packages.keys():
+ packages[k] = {d for d in packages[k] if d in pkg_names}
+
+ return packages
+
+
+def add_package_runtime_dependencies(path, packages):
+ """
+ Check the path and if it exists extract the packages runtime dependencies.
+
+ :param Path path: The resource file containing the runtime dependencies
+ :param dict packages: A mapping from package names to the sets of runtime
+ dependencies to add to
+ """
+ content = path.read_text()
+ dependencies = set(content.split(os.pathsep) if content else [])
+ packages[path.name] = dependencies
+
+
+def order_packages(packages):
+ """
+ Order packages topologically.
+
+ :param dict packages: A mapping from package name to the set of runtime
+ dependencies
+ :returns: The package names
+ :rtype: list
+ """
+ # select packages with no dependencies in alphabetical order
+ to_be_ordered = list(packages.keys())
+ ordered = []
+ while to_be_ordered:
+ pkg_names_without_deps = [
+ name for name in to_be_ordered if not packages[name]]
+ if not pkg_names_without_deps:
+ reduce_cycle_set(packages)
+ raise RuntimeError(
+ 'Circular dependency between: ' + ', '.join(sorted(packages)))
+ pkg_names_without_deps.sort()
+ pkg_name = pkg_names_without_deps[0]
+ to_be_ordered.remove(pkg_name)
+ ordered.append(pkg_name)
+ # remove item from dependency lists
+ for k in list(packages.keys()):
+ if pkg_name in packages[k]:
+ packages[k].remove(pkg_name)
+ return ordered
+
+
+def reduce_cycle_set(packages):
+ """
+ Reduce the set of packages to the ones part of the circular dependency.
+
+ :param dict packages: A mapping from package name to the set of runtime
+ dependencies which is modified in place
+ """
+ last_depended = None
+ while len(packages) > 0:
+ # get all remaining dependencies
+ depended = set()
+ for pkg_name, dependencies in packages.items():
+ depended = depended.union(dependencies)
+ # remove all packages which are not dependent on
+ for name in list(packages.keys()):
+ if name not in depended:
+ del packages[name]
+ if last_depended:
+ # if remaining packages haven't changed return them
+ if last_depended == depended:
+ return packages.keys()
+ # otherwise reduce again
+ last_depended = depended
+
+
+def _include_comments():
+ # skipping comment lines when COLCON_TRACE is not set speeds up the
+ # processing especially on Windows
+ return bool(os.environ.get('COLCON_TRACE'))
+
+
+def get_commands(pkg_name, prefix, primary_extension, additional_extension):
+ commands = []
+ package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv')
+ if os.path.exists(package_dsv_path):
+ commands += process_dsv_file(
+ package_dsv_path, prefix, primary_extension, additional_extension)
+ return commands
+
+
+def process_dsv_file(
+ dsv_path, prefix, primary_extension=None, additional_extension=None
+):
+ commands = []
+ if _include_comments():
+ commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path}))
+ with open(dsv_path, 'r') as h:
+ content = h.read()
+ lines = content.splitlines()
+
+ basenames = OrderedDict()
+ for i, line in enumerate(lines):
+ # skip over empty or whitespace-only lines
+ if not line.strip():
+ continue
+ # skip over comments
+ if line.startswith('#'):
+ continue
+ try:
+ type_, remainder = line.split(';', 1)
+ except ValueError:
+ raise RuntimeError(
+ "Line %d in '%s' doesn't contain a semicolon separating the "
+ 'type from the arguments' % (i + 1, dsv_path))
+ if type_ != DSV_TYPE_SOURCE:
+ # handle non-source lines
+ try:
+ commands += handle_dsv_types_except_source(
+ type_, remainder, prefix)
+ except RuntimeError as e:
+ raise RuntimeError(
+ "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e
+ else:
+ # group remaining source lines by basename
+ path_without_ext, ext = os.path.splitext(remainder)
+ if path_without_ext not in basenames:
+ basenames[path_without_ext] = set()
+ assert ext.startswith('.')
+ ext = ext[1:]
+ if ext in (primary_extension, additional_extension):
+ basenames[path_without_ext].add(ext)
+
+ # add the dsv extension to each basename if the file exists
+ for basename, extensions in basenames.items():
+ if not os.path.isabs(basename):
+ basename = os.path.join(prefix, basename)
+ if os.path.exists(basename + '.dsv'):
+ extensions.add('dsv')
+
+ for basename, extensions in basenames.items():
+ if not os.path.isabs(basename):
+ basename = os.path.join(prefix, basename)
+ if 'dsv' in extensions:
+ # process dsv files recursively
+ commands += process_dsv_file(
+ basename + '.dsv', prefix, primary_extension=primary_extension,
+ additional_extension=additional_extension)
+ elif primary_extension in extensions and len(extensions) == 1:
+ # source primary-only files
+ commands += [
+ FORMAT_STR_INVOKE_SCRIPT.format_map({
+ 'prefix': prefix,
+ 'script_path': basename + '.' + primary_extension})]
+ elif additional_extension in extensions:
+ # source non-primary files
+ commands += [
+ FORMAT_STR_INVOKE_SCRIPT.format_map({
+ 'prefix': prefix,
+ 'script_path': basename + '.' + additional_extension})]
+
+ return commands
+
+
+def handle_dsv_types_except_source(type_, remainder, prefix):
+ commands = []
+ if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET):
+ try:
+ env_name, value = remainder.split(';', 1)
+ except ValueError:
+ raise RuntimeError(
+ "doesn't contain a semicolon separating the environment name "
+ 'from the value')
+ try_prefixed_value = os.path.join(prefix, value) if value else prefix
+ if os.path.exists(try_prefixed_value):
+ value = try_prefixed_value
+ if type_ == DSV_TYPE_SET:
+ commands += _set(env_name, value)
+ elif type_ == DSV_TYPE_SET_IF_UNSET:
+ commands += _set_if_unset(env_name, value)
+ else:
+ assert False
+ elif type_ in (
+ DSV_TYPE_APPEND_NON_DUPLICATE,
+ DSV_TYPE_PREPEND_NON_DUPLICATE,
+ DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS
+ ):
+ try:
+ env_name_and_values = remainder.split(';')
+ except ValueError:
+ raise RuntimeError(
+ "doesn't contain a semicolon separating the environment name "
+ 'from the values')
+ env_name = env_name_and_values[0]
+ values = env_name_and_values[1:]
+ for value in values:
+ if not value:
+ value = prefix
+ elif not os.path.isabs(value):
+ value = os.path.join(prefix, value)
+ if (
+ type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and
+ not os.path.exists(value)
+ ):
+ comment = f'skip extending {env_name} with not existing ' \
+ f'path: {value}'
+ if _include_comments():
+ commands.append(
+ FORMAT_STR_COMMENT_LINE.format_map({'comment': comment}))
+ elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE:
+ commands += _append_unique_value(env_name, value)
+ else:
+ commands += _prepend_unique_value(env_name, value)
+ else:
+ raise RuntimeError(
+ 'contains an unknown environment hook type: ' + type_)
+ return commands
+
+
+env_state = {}
+
+
+def _append_unique_value(name, value):
+ global env_state
+ if name not in env_state:
+ if os.environ.get(name):
+ env_state[name] = set(os.environ[name].split(os.pathsep))
+ else:
+ env_state[name] = set()
+ # append even if the variable has not been set yet, in case a shell script sets the
+ # same variable without the knowledge of this Python script.
+ # later _remove_ending_separators() will cleanup any unintentional leading separator
+ extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': extend + value})
+ if value not in env_state[name]:
+ env_state[name].add(value)
+ else:
+ if not _include_comments():
+ return []
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+def _prepend_unique_value(name, value):
+ global env_state
+ if name not in env_state:
+ if os.environ.get(name):
+ env_state[name] = set(os.environ[name].split(os.pathsep))
+ else:
+ env_state[name] = set()
+ # prepend even if the variable has not been set yet, in case a shell script sets the
+ # same variable without the knowledge of this Python script.
+ # later _remove_ending_separators() will cleanup any unintentional trailing separator
+ extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name})
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value + extend})
+ if value not in env_state[name]:
+ env_state[name].add(value)
+ else:
+ if not _include_comments():
+ return []
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+# generate commands for removing prepended underscores
+def _remove_ending_separators():
+ # do nothing if the shell extension does not implement the logic
+ if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None:
+ return []
+
+ global env_state
+ commands = []
+ for name in env_state:
+ # skip variables that already had values before this script started prepending
+ if name in os.environ:
+ continue
+ commands += [
+ FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}),
+ FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})]
+ return commands
+
+
+def _set(name, value):
+ global env_state
+ env_state[name] = value
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value})
+ return [line]
+
+
+def _set_if_unset(name, value):
+ global env_state
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value})
+ if env_state.get(name, os.environ.get(name)):
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+if __name__ == '__main__': # pragma: no cover
+ try:
+ rc = main()
+ except RuntimeError as e:
+ print(str(e), file=sys.stderr)
+ rc = 1
+ sys.exit(rc)
diff --git a/install/_local_setup_util_sh.py b/install/_local_setup_util_sh.py
new file mode 100644
index 00000000..f67eaa98
--- /dev/null
+++ b/install/_local_setup_util_sh.py
@@ -0,0 +1,407 @@
+# Copyright 2016-2019 Dirk Thomas
+# Licensed under the Apache License, Version 2.0
+
+import argparse
+from collections import OrderedDict
+import os
+from pathlib import Path
+import sys
+
+
+FORMAT_STR_COMMENT_LINE = '# {comment}'
+FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"'
+FORMAT_STR_USE_ENV_VAR = '${name}'
+FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' # noqa: E501
+FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501
+FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501
+
+DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate'
+DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate'
+DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists'
+DSV_TYPE_SET = 'set'
+DSV_TYPE_SET_IF_UNSET = 'set-if-unset'
+DSV_TYPE_SOURCE = 'source'
+
+
+def main(argv=sys.argv[1:]): # noqa: D103
+ parser = argparse.ArgumentParser(
+ description='Output shell commands for the packages in topological '
+ 'order')
+ parser.add_argument(
+ 'primary_extension',
+ help='The file extension of the primary shell')
+ parser.add_argument(
+ 'additional_extension', nargs='?',
+ help='The additional file extension to be considered')
+ parser.add_argument(
+ '--merged-install', action='store_true',
+ help='All install prefixes are merged into a single location')
+ args = parser.parse_args(argv)
+
+ packages = get_packages(Path(__file__).parent, args.merged_install)
+
+ ordered_packages = order_packages(packages)
+ for pkg_name in ordered_packages:
+ if _include_comments():
+ print(
+ FORMAT_STR_COMMENT_LINE.format_map(
+ {'comment': 'Package: ' + pkg_name}))
+ prefix = os.path.abspath(os.path.dirname(__file__))
+ if not args.merged_install:
+ prefix = os.path.join(prefix, pkg_name)
+ for line in get_commands(
+ pkg_name, prefix, args.primary_extension,
+ args.additional_extension
+ ):
+ print(line)
+
+ for line in _remove_ending_separators():
+ print(line)
+
+
+def get_packages(prefix_path, merged_install):
+ """
+ Find packages based on colcon-specific files created during installation.
+
+ :param Path prefix_path: The install prefix path of all packages
+ :param bool merged_install: The flag if the packages are all installed
+ directly in the prefix or if each package is installed in a subdirectory
+ named after the package
+ :returns: A mapping from the package name to the set of runtime
+ dependencies
+ :rtype: dict
+ """
+ packages = {}
+ # since importing colcon_core isn't feasible here the following constant
+ # must match colcon_core.location.get_relative_package_index_path()
+ subdirectory = 'share/colcon-core/packages'
+ if merged_install:
+ # return if workspace is empty
+ if not (prefix_path / subdirectory).is_dir():
+ return packages
+ # find all files in the subdirectory
+ for p in (prefix_path / subdirectory).iterdir():
+ if not p.is_file():
+ continue
+ if p.name.startswith('.'):
+ continue
+ add_package_runtime_dependencies(p, packages)
+ else:
+ # for each subdirectory look for the package specific file
+ for p in prefix_path.iterdir():
+ if not p.is_dir():
+ continue
+ if p.name.startswith('.'):
+ continue
+ p = p / subdirectory / p.name
+ if p.is_file():
+ add_package_runtime_dependencies(p, packages)
+
+ # remove unknown dependencies
+ pkg_names = set(packages.keys())
+ for k in packages.keys():
+ packages[k] = {d for d in packages[k] if d in pkg_names}
+
+ return packages
+
+
+def add_package_runtime_dependencies(path, packages):
+ """
+ Check the path and if it exists extract the packages runtime dependencies.
+
+ :param Path path: The resource file containing the runtime dependencies
+ :param dict packages: A mapping from package names to the sets of runtime
+ dependencies to add to
+ """
+ content = path.read_text()
+ dependencies = set(content.split(os.pathsep) if content else [])
+ packages[path.name] = dependencies
+
+
+def order_packages(packages):
+ """
+ Order packages topologically.
+
+ :param dict packages: A mapping from package name to the set of runtime
+ dependencies
+ :returns: The package names
+ :rtype: list
+ """
+ # select packages with no dependencies in alphabetical order
+ to_be_ordered = list(packages.keys())
+ ordered = []
+ while to_be_ordered:
+ pkg_names_without_deps = [
+ name for name in to_be_ordered if not packages[name]]
+ if not pkg_names_without_deps:
+ reduce_cycle_set(packages)
+ raise RuntimeError(
+ 'Circular dependency between: ' + ', '.join(sorted(packages)))
+ pkg_names_without_deps.sort()
+ pkg_name = pkg_names_without_deps[0]
+ to_be_ordered.remove(pkg_name)
+ ordered.append(pkg_name)
+ # remove item from dependency lists
+ for k in list(packages.keys()):
+ if pkg_name in packages[k]:
+ packages[k].remove(pkg_name)
+ return ordered
+
+
+def reduce_cycle_set(packages):
+ """
+ Reduce the set of packages to the ones part of the circular dependency.
+
+ :param dict packages: A mapping from package name to the set of runtime
+ dependencies which is modified in place
+ """
+ last_depended = None
+ while len(packages) > 0:
+ # get all remaining dependencies
+ depended = set()
+ for pkg_name, dependencies in packages.items():
+ depended = depended.union(dependencies)
+ # remove all packages which are not dependent on
+ for name in list(packages.keys()):
+ if name not in depended:
+ del packages[name]
+ if last_depended:
+ # if remaining packages haven't changed return them
+ if last_depended == depended:
+ return packages.keys()
+ # otherwise reduce again
+ last_depended = depended
+
+
+def _include_comments():
+ # skipping comment lines when COLCON_TRACE is not set speeds up the
+ # processing especially on Windows
+ return bool(os.environ.get('COLCON_TRACE'))
+
+
+def get_commands(pkg_name, prefix, primary_extension, additional_extension):
+ commands = []
+ package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv')
+ if os.path.exists(package_dsv_path):
+ commands += process_dsv_file(
+ package_dsv_path, prefix, primary_extension, additional_extension)
+ return commands
+
+
+def process_dsv_file(
+ dsv_path, prefix, primary_extension=None, additional_extension=None
+):
+ commands = []
+ if _include_comments():
+ commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path}))
+ with open(dsv_path, 'r') as h:
+ content = h.read()
+ lines = content.splitlines()
+
+ basenames = OrderedDict()
+ for i, line in enumerate(lines):
+ # skip over empty or whitespace-only lines
+ if not line.strip():
+ continue
+ # skip over comments
+ if line.startswith('#'):
+ continue
+ try:
+ type_, remainder = line.split(';', 1)
+ except ValueError:
+ raise RuntimeError(
+ "Line %d in '%s' doesn't contain a semicolon separating the "
+ 'type from the arguments' % (i + 1, dsv_path))
+ if type_ != DSV_TYPE_SOURCE:
+ # handle non-source lines
+ try:
+ commands += handle_dsv_types_except_source(
+ type_, remainder, prefix)
+ except RuntimeError as e:
+ raise RuntimeError(
+ "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e
+ else:
+ # group remaining source lines by basename
+ path_without_ext, ext = os.path.splitext(remainder)
+ if path_without_ext not in basenames:
+ basenames[path_without_ext] = set()
+ assert ext.startswith('.')
+ ext = ext[1:]
+ if ext in (primary_extension, additional_extension):
+ basenames[path_without_ext].add(ext)
+
+ # add the dsv extension to each basename if the file exists
+ for basename, extensions in basenames.items():
+ if not os.path.isabs(basename):
+ basename = os.path.join(prefix, basename)
+ if os.path.exists(basename + '.dsv'):
+ extensions.add('dsv')
+
+ for basename, extensions in basenames.items():
+ if not os.path.isabs(basename):
+ basename = os.path.join(prefix, basename)
+ if 'dsv' in extensions:
+ # process dsv files recursively
+ commands += process_dsv_file(
+ basename + '.dsv', prefix, primary_extension=primary_extension,
+ additional_extension=additional_extension)
+ elif primary_extension in extensions and len(extensions) == 1:
+ # source primary-only files
+ commands += [
+ FORMAT_STR_INVOKE_SCRIPT.format_map({
+ 'prefix': prefix,
+ 'script_path': basename + '.' + primary_extension})]
+ elif additional_extension in extensions:
+ # source non-primary files
+ commands += [
+ FORMAT_STR_INVOKE_SCRIPT.format_map({
+ 'prefix': prefix,
+ 'script_path': basename + '.' + additional_extension})]
+
+ return commands
+
+
+def handle_dsv_types_except_source(type_, remainder, prefix):
+ commands = []
+ if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET):
+ try:
+ env_name, value = remainder.split(';', 1)
+ except ValueError:
+ raise RuntimeError(
+ "doesn't contain a semicolon separating the environment name "
+ 'from the value')
+ try_prefixed_value = os.path.join(prefix, value) if value else prefix
+ if os.path.exists(try_prefixed_value):
+ value = try_prefixed_value
+ if type_ == DSV_TYPE_SET:
+ commands += _set(env_name, value)
+ elif type_ == DSV_TYPE_SET_IF_UNSET:
+ commands += _set_if_unset(env_name, value)
+ else:
+ assert False
+ elif type_ in (
+ DSV_TYPE_APPEND_NON_DUPLICATE,
+ DSV_TYPE_PREPEND_NON_DUPLICATE,
+ DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS
+ ):
+ try:
+ env_name_and_values = remainder.split(';')
+ except ValueError:
+ raise RuntimeError(
+ "doesn't contain a semicolon separating the environment name "
+ 'from the values')
+ env_name = env_name_and_values[0]
+ values = env_name_and_values[1:]
+ for value in values:
+ if not value:
+ value = prefix
+ elif not os.path.isabs(value):
+ value = os.path.join(prefix, value)
+ if (
+ type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and
+ not os.path.exists(value)
+ ):
+ comment = f'skip extending {env_name} with not existing ' \
+ f'path: {value}'
+ if _include_comments():
+ commands.append(
+ FORMAT_STR_COMMENT_LINE.format_map({'comment': comment}))
+ elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE:
+ commands += _append_unique_value(env_name, value)
+ else:
+ commands += _prepend_unique_value(env_name, value)
+ else:
+ raise RuntimeError(
+ 'contains an unknown environment hook type: ' + type_)
+ return commands
+
+
+env_state = {}
+
+
+def _append_unique_value(name, value):
+ global env_state
+ if name not in env_state:
+ if os.environ.get(name):
+ env_state[name] = set(os.environ[name].split(os.pathsep))
+ else:
+ env_state[name] = set()
+ # append even if the variable has not been set yet, in case a shell script sets the
+ # same variable without the knowledge of this Python script.
+ # later _remove_ending_separators() will cleanup any unintentional leading separator
+ extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': extend + value})
+ if value not in env_state[name]:
+ env_state[name].add(value)
+ else:
+ if not _include_comments():
+ return []
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+def _prepend_unique_value(name, value):
+ global env_state
+ if name not in env_state:
+ if os.environ.get(name):
+ env_state[name] = set(os.environ[name].split(os.pathsep))
+ else:
+ env_state[name] = set()
+ # prepend even if the variable has not been set yet, in case a shell script sets the
+ # same variable without the knowledge of this Python script.
+ # later _remove_ending_separators() will cleanup any unintentional trailing separator
+ extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name})
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value + extend})
+ if value not in env_state[name]:
+ env_state[name].add(value)
+ else:
+ if not _include_comments():
+ return []
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+# generate commands for removing prepended underscores
+def _remove_ending_separators():
+ # do nothing if the shell extension does not implement the logic
+ if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None:
+ return []
+
+ global env_state
+ commands = []
+ for name in env_state:
+ # skip variables that already had values before this script started prepending
+ if name in os.environ:
+ continue
+ commands += [
+ FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}),
+ FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})]
+ return commands
+
+
+def _set(name, value):
+ global env_state
+ env_state[name] = value
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value})
+ return [line]
+
+
+def _set_if_unset(name, value):
+ global env_state
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value})
+ if env_state.get(name, os.environ.get(name)):
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+if __name__ == '__main__': # pragma: no cover
+ try:
+ rc = main()
+ except RuntimeError as e:
+ print(str(e), file=sys.stderr)
+ rc = 1
+ sys.exit(rc)
diff --git a/install/autonomous_kart/share/ament_index/resource_index/packages/autonomous_kart b/install/autonomous_kart/share/ament_index/resource_index/packages/autonomous_kart
new file mode 100644
index 00000000..e69de29b
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.dsv b/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.dsv
new file mode 100644
index 00000000..79d4c95b
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.dsv
@@ -0,0 +1 @@
+prepend-non-duplicate;AMENT_PREFIX_PATH;
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.ps1 b/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.ps1
new file mode 100644
index 00000000..26b99975
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.ps1
@@ -0,0 +1,3 @@
+# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
+
+colcon_prepend_unique_value AMENT_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX"
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.sh b/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.sh
new file mode 100644
index 00000000..f3041f68
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.sh
@@ -0,0 +1,3 @@
+# generated from colcon_core/shell/template/hook_prepend_value.sh.em
+
+_colcon_prepend_unique_value AMENT_PREFIX_PATH "$COLCON_CURRENT_PREFIX"
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.dsv b/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.dsv
new file mode 100644
index 00000000..257067d4
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.dsv
@@ -0,0 +1 @@
+prepend-non-duplicate;PYTHONPATH;lib/python3.10/site-packages
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.ps1 b/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.ps1
new file mode 100644
index 00000000..caffe83f
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.ps1
@@ -0,0 +1,3 @@
+# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
+
+colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX\lib/python3.10/site-packages"
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.sh b/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.sh
new file mode 100644
index 00000000..660c3483
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.sh
@@ -0,0 +1,3 @@
+# generated from colcon_core/shell/template/hook_prepend_value.sh.em
+
+_colcon_prepend_unique_value PYTHONPATH "$COLCON_CURRENT_PREFIX/lib/python3.10/site-packages"
diff --git a/install/autonomous_kart/share/autonomous_kart/launch/bringup_pi.launch.py b/install/autonomous_kart/share/autonomous_kart/launch/bringup_pi.launch.py
new file mode 100644
index 00000000..88a2aff8
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/launch/bringup_pi.launch.py
@@ -0,0 +1,49 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import os
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+ pkg_share = get_package_share_directory('autonomous_kart')
+
+ return LaunchDescription([
+ Node(
+ package='autonomous_kart',
+ executable='motor_node',
+ name='motor_node',
+ parameters=[os.path.join(pkg_share, 'params', 'controller.yaml'), {'simulation_mode': False}]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='steering_node',
+ name='steering_node',
+ parameters=[os.path.join(pkg_share, 'params', 'controller.yaml')]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='camera_node',
+ name='camera_node',
+ parameters=[os.path.join(pkg_share, 'params', 'camera.yaml')]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='gps_node',
+ name='gps_node',
+ parameters=[os.path.join(pkg_share, 'params', 'gps.yaml')]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='pathfinder_node',
+ name='pathfinder_node',
+ parameters=[os.path.join(pkg_share, 'params', 'planner.yaml'),
+ os.path.join(pkg_share, 'params', 'safety.yaml'), os.path.join(pkg_share, 'params', 'gps.yaml')]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='opencv_pathfinder_node',
+ name='opencv_pathfinder_node',
+ # parameters=[os.path.join(pkg_share, 'params', 'planner.yaml'),
+ # os.path.join(pkg_share, 'params', 'safety.yaml'), os.path.join(pkg_share, 'params', 'gps.yaml')]
+ ),
+ ])
diff --git a/install/autonomous_kart/share/autonomous_kart/launch/bringup_sim.launch.py b/install/autonomous_kart/share/autonomous_kart/launch/bringup_sim.launch.py
new file mode 100644
index 00000000..3e8f0212
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/launch/bringup_sim.launch.py
@@ -0,0 +1,45 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import os
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+ pkg_share = get_package_share_directory('autonomous_kart')
+
+ return LaunchDescription([
+ Node(
+ package='autonomous_kart',
+ executable='motor_node',
+ name='motor_node',
+ parameters=[os.path.join(pkg_share, 'params', 'controller.yaml'), {'simulation_mode': True}]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='steering_node',
+ name='steering_node',
+ parameters=[os.path.join(pkg_share, 'params', 'controller.yaml'), {'simulation_mode': True}]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='camera_node',
+ name='camera_node',
+ parameters=[os.path.join(pkg_share, 'params', 'camera.yaml'), {'simulation_mode': True}]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='pathfinder_node',
+ name='pathfinder_node',
+ parameters=[os.path.join(pkg_share, 'params', 'planner.yaml'),
+ os.path.join(pkg_share, 'params', 'safety.yaml'), os.path.join(pkg_share, 'params', 'gps.yaml'),
+ {'simulation_mode': True}]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='opencv_pathfinder_node',
+ name='opencv_pathfinder_node',
+ parameters=[os.path.join(pkg_share, 'params', 'planner.yaml'),
+ os.path.join(pkg_share, 'params', 'safety.yaml'), os.path.join(pkg_share, 'params', 'gps.yaml'),
+ {'simulation_mode': True}]
+ ),
+ ])
diff --git a/install/autonomous_kart/share/autonomous_kart/package.bash b/install/autonomous_kart/share/autonomous_kart/package.bash
new file mode 100644
index 00000000..48271952
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/package.bash
@@ -0,0 +1,31 @@
+# generated from colcon_bash/shell/template/package.bash.em
+
+# This script extends the environment for this package.
+
+# a bash script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ # the prefix is two levels up from the package specific share directory
+ _colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)"
+else
+ _colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_bash_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$@"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# source sh script of this package
+_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/autonomous_kart/package.sh"
+
+unset _colcon_package_bash_source_script
+unset _colcon_package_bash_COLCON_CURRENT_PREFIX
diff --git a/install/autonomous_kart/share/autonomous_kart/package.dsv b/install/autonomous_kart/share/autonomous_kart/package.dsv
new file mode 100644
index 00000000..b9a0734d
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/package.dsv
@@ -0,0 +1,6 @@
+source;share/autonomous_kart/hook/pythonpath.ps1
+source;share/autonomous_kart/hook/pythonpath.dsv
+source;share/autonomous_kart/hook/pythonpath.sh
+source;share/autonomous_kart/hook/ament_prefix_path.ps1
+source;share/autonomous_kart/hook/ament_prefix_path.dsv
+source;share/autonomous_kart/hook/ament_prefix_path.sh
diff --git a/install/autonomous_kart/share/autonomous_kart/package.ps1 b/install/autonomous_kart/share/autonomous_kart/package.ps1
new file mode 100644
index 00000000..3871215d
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/package.ps1
@@ -0,0 +1,116 @@
+# generated from colcon_powershell/shell/template/package.ps1.em
+
+# function to append a value to a variable
+# which uses colons as separators
+# duplicates as well as leading separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+function colcon_append_unique_value {
+ param (
+ $_listname,
+ $_value
+ )
+
+ # get values from variable
+ if (Test-Path Env:$_listname) {
+ $_values=(Get-Item env:$_listname).Value
+ } else {
+ $_values=""
+ }
+ $_duplicate=""
+ # start with no values
+ $_all_values=""
+ # iterate over existing values in the variable
+ if ($_values) {
+ $_values.Split(";") | ForEach {
+ # not an empty string
+ if ($_) {
+ # not a duplicate of _value
+ if ($_ -eq $_value) {
+ $_duplicate="1"
+ }
+ if ($_all_values) {
+ $_all_values="${_all_values};$_"
+ } else {
+ $_all_values="$_"
+ }
+ }
+ }
+ }
+ # append only non-duplicates
+ if (!$_duplicate) {
+ # avoid leading separator
+ if ($_all_values) {
+ $_all_values="${_all_values};${_value}"
+ } else {
+ $_all_values="${_value}"
+ }
+ }
+
+ # export the updated variable
+ Set-Item env:\$_listname -Value "$_all_values"
+}
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+function colcon_prepend_unique_value {
+ param (
+ $_listname,
+ $_value
+ )
+
+ # get values from variable
+ if (Test-Path Env:$_listname) {
+ $_values=(Get-Item env:$_listname).Value
+ } else {
+ $_values=""
+ }
+ # start with the new value
+ $_all_values="$_value"
+ # iterate over existing values in the variable
+ if ($_values) {
+ $_values.Split(";") | ForEach {
+ # not an empty string
+ if ($_) {
+ # not a duplicate of _value
+ if ($_ -ne $_value) {
+ # keep non-duplicate values
+ $_all_values="${_all_values};$_"
+ }
+ }
+ }
+ }
+ # export the updated variable
+ Set-Item env:\$_listname -Value "$_all_values"
+}
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+function colcon_package_source_powershell_script {
+ param (
+ $_colcon_package_source_powershell_script
+ )
+ # source script with conditional trace output
+ if (Test-Path $_colcon_package_source_powershell_script) {
+ if ($env:COLCON_TRACE) {
+ echo ". '$_colcon_package_source_powershell_script'"
+ }
+ . "$_colcon_package_source_powershell_script"
+ } else {
+ Write-Error "not found: '$_colcon_package_source_powershell_script'"
+ }
+}
+
+
+# a powershell script is able to determine its own path
+# the prefix is two levels up from the package specific share directory
+$env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.FullName
+
+colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/autonomous_kart/hook/pythonpath.ps1"
+colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/autonomous_kart/hook/ament_prefix_path.ps1"
+
+Remove-Item Env:\COLCON_CURRENT_PREFIX
diff --git a/install/autonomous_kart/share/autonomous_kart/package.sh b/install/autonomous_kart/share/autonomous_kart/package.sh
new file mode 100644
index 00000000..d53dab10
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/package.sh
@@ -0,0 +1,87 @@
+# generated from colcon_core/shell/template/package.sh.em
+
+# This script extends the environment for this package.
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prepend_unique_value() {
+ # arguments
+ _listname="$1"
+ _value="$2"
+
+ # get values from variable
+ eval _values=\"\$$_listname\"
+ # backup the field separator
+ _colcon_prepend_unique_value_IFS=$IFS
+ IFS=":"
+ # start with the new value
+ _all_values="$_value"
+ # workaround SH_WORD_SPLIT not being set in zsh
+ if [ "$(command -v colcon_zsh_convert_to_array)" ]; then
+ colcon_zsh_convert_to_array _values
+ fi
+ # iterate over existing values in the variable
+ for _item in $_values; do
+ # ignore empty strings
+ if [ -z "$_item" ]; then
+ continue
+ fi
+ # ignore duplicates of _value
+ if [ "$_item" = "$_value" ]; then
+ continue
+ fi
+ # keep non-duplicate values
+ _all_values="$_all_values:$_item"
+ done
+ unset _item
+ # restore the field separator
+ IFS=$_colcon_prepend_unique_value_IFS
+ unset _colcon_prepend_unique_value_IFS
+ # export the updated variable
+ eval export $_listname=\"$_all_values\"
+ unset _all_values
+ unset _values
+
+ unset _value
+ unset _listname
+}
+
+# since a plain shell script can't determine its own path when being sourced
+# either use the provided COLCON_CURRENT_PREFIX
+# or fall back to the build time prefix (if it exists)
+_colcon_package_sh_COLCON_CURRENT_PREFIX="/ws/install/autonomous_kart"
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then
+ echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
+ unset _colcon_package_sh_COLCON_CURRENT_PREFIX
+ return 1
+ fi
+ COLCON_CURRENT_PREFIX="$_colcon_package_sh_COLCON_CURRENT_PREFIX"
+fi
+unset _colcon_package_sh_COLCON_CURRENT_PREFIX
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_sh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$@"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# source sh hooks
+_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/autonomous_kart/hook/pythonpath.sh"
+_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/autonomous_kart/hook/ament_prefix_path.sh"
+
+unset _colcon_package_sh_source_script
+unset COLCON_CURRENT_PREFIX
+
+# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks
diff --git a/install/autonomous_kart/share/autonomous_kart/package.xml b/install/autonomous_kart/share/autonomous_kart/package.xml
new file mode 100644
index 00000000..0005e101
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/package.xml
@@ -0,0 +1,23 @@
+
+
+
+ autonomous_kart
+ 0.0.1
+ Package containing all nodes for driving in different states.
+ root
+ Apache-2.0
+
+ ament_python
+
+ rclpy
+ geometry_msgs
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
\ No newline at end of file
diff --git a/install/autonomous_kart/share/autonomous_kart/package.zsh b/install/autonomous_kart/share/autonomous_kart/package.zsh
new file mode 100644
index 00000000..179a08f0
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/package.zsh
@@ -0,0 +1,42 @@
+# generated from colcon_zsh/shell/template/package.zsh.em
+
+# This script extends the environment for this package.
+
+# a zsh script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ # the prefix is two levels up from the package specific share directory
+ _colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)"
+else
+ _colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_zsh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$@"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# function to convert array-like strings into arrays
+# to workaround SH_WORD_SPLIT not being set
+colcon_zsh_convert_to_array() {
+ local _listname=$1
+ local _dollar="$"
+ local _split="{="
+ local _to_array="(\"$_dollar$_split$_listname}\")"
+ eval $_listname=$_to_array
+}
+
+# source sh script of this package
+_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/autonomous_kart/package.sh"
+unset convert_zsh_to_array
+
+unset _colcon_package_zsh_source_script
+unset _colcon_package_zsh_COLCON_CURRENT_PREFIX
diff --git a/install/autonomous_kart/share/autonomous_kart/params/camera.yaml b/install/autonomous_kart/share/autonomous_kart/params/camera.yaml
new file mode 100644
index 00000000..346227a4
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/params/camera.yaml
@@ -0,0 +1,5 @@
+# Camera config settings
+camera_node:
+ ros__parameters:
+ simulation_mode: false # set later
+ fps: 30.0 # float
\ No newline at end of file
diff --git a/install/autonomous_kart/share/autonomous_kart/params/controller.yaml b/install/autonomous_kart/share/autonomous_kart/params/controller.yaml
new file mode 100644
index 00000000..3bd8bf1a
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/params/controller.yaml
@@ -0,0 +1,8 @@
+# Params and toggles for RC controller drive including gain, rates, etc.
+# Dummy values for now
+motor_node:
+ ros__parameters:
+ max_linear_speed: 5.0 # m/s
+ max_angular_speed: 3.0 # rad/s
+ motor_timeout: 1.0 # seconds
+ simulation_mode: false # set in motor node
\ No newline at end of file
diff --git a/install/autonomous_kart/share/autonomous_kart/params/gps.yaml b/install/autonomous_kart/share/autonomous_kart/params/gps.yaml
new file mode 100644
index 00000000..e69de29b
diff --git a/install/autonomous_kart/share/autonomous_kart/params/planner.yaml b/install/autonomous_kart/share/autonomous_kart/params/planner.yaml
new file mode 100644
index 00000000..17ee3bcd
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/params/planner.yaml
@@ -0,0 +1 @@
+# Planner configs including perception thresholds, toggles, etc.
\ No newline at end of file
diff --git a/install/autonomous_kart/share/autonomous_kart/params/safety.yaml b/install/autonomous_kart/share/autonomous_kart/params/safety.yaml
new file mode 100644
index 00000000..39564898
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/params/safety.yaml
@@ -0,0 +1 @@
+# Safety limits and estop config
\ No newline at end of file
diff --git a/install/autonomous_kart/share/colcon-core/packages/autonomous_kart b/install/autonomous_kart/share/colcon-core/packages/autonomous_kart
new file mode 100644
index 00000000..18e15c42
--- /dev/null
+++ b/install/autonomous_kart/share/colcon-core/packages/autonomous_kart
@@ -0,0 +1 @@
+geometry_msgs:rclpy
\ No newline at end of file
diff --git a/install/local_setup.bash b/install/local_setup.bash
new file mode 100644
index 00000000..03f00256
--- /dev/null
+++ b/install/local_setup.bash
@@ -0,0 +1,121 @@
+# generated from colcon_bash/shell/template/prefix.bash.em
+
+# This script extends the environment with all packages contained in this
+# prefix path.
+
+# a bash script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)"
+else
+ _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prefix_bash_prepend_unique_value() {
+ # arguments
+ _listname="$1"
+ _value="$2"
+
+ # get values from variable
+ eval _values=\"\$$_listname\"
+ # backup the field separator
+ _colcon_prefix_bash_prepend_unique_value_IFS="$IFS"
+ IFS=":"
+ # start with the new value
+ _all_values="$_value"
+ _contained_value=""
+ # iterate over existing values in the variable
+ for _item in $_values; do
+ # ignore empty strings
+ if [ -z "$_item" ]; then
+ continue
+ fi
+ # ignore duplicates of _value
+ if [ "$_item" = "$_value" ]; then
+ _contained_value=1
+ continue
+ fi
+ # keep non-duplicate values
+ _all_values="$_all_values:$_item"
+ done
+ unset _item
+ if [ -z "$_contained_value" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ if [ "$_all_values" = "$_value" ]; then
+ echo "export $_listname=$_value"
+ else
+ echo "export $_listname=$_value:\$$_listname"
+ fi
+ fi
+ fi
+ unset _contained_value
+ # restore the field separator
+ IFS="$_colcon_prefix_bash_prepend_unique_value_IFS"
+ unset _colcon_prefix_bash_prepend_unique_value_IFS
+ # export the updated variable
+ eval export $_listname=\"$_all_values\"
+ unset _all_values
+ unset _values
+
+ unset _value
+ unset _listname
+}
+
+# add this prefix to the COLCON_PREFIX_PATH
+_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX"
+unset _colcon_prefix_bash_prepend_unique_value
+
+# check environment variable for custom Python executable
+if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then
+ if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then
+ echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist"
+ return 1
+ fi
+ _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE"
+else
+ # try the Python executable known at configure time
+ _colcon_python_executable="/usr/bin/python3"
+ # if it doesn't exist try a fall back
+ if [ ! -f "$_colcon_python_executable" ]; then
+ if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then
+ echo "error: unable to find python3 executable"
+ return 1
+ fi
+ _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"`
+ fi
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_sh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# get all commands in topological order
+_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)"
+unset _colcon_python_executable
+if [ -n "$COLCON_TRACE" ]; then
+ echo "$(declare -f _colcon_prefix_sh_source_script)"
+ echo "# Execute generated script:"
+ echo "# <<<"
+ echo "${_colcon_ordered_commands}"
+ echo "# >>>"
+ echo "unset _colcon_prefix_sh_source_script"
+fi
+eval "${_colcon_ordered_commands}"
+unset _colcon_ordered_commands
+
+unset _colcon_prefix_sh_source_script
+
+unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX
diff --git a/install/local_setup.ps1 b/install/local_setup.ps1
new file mode 100644
index 00000000..6f68c8de
--- /dev/null
+++ b/install/local_setup.ps1
@@ -0,0 +1,55 @@
+# generated from colcon_powershell/shell/template/prefix.ps1.em
+
+# This script extends the environment with all packages contained in this
+# prefix path.
+
+# check environment variable for custom Python executable
+if ($env:COLCON_PYTHON_EXECUTABLE) {
+ if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) {
+ echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist"
+ exit 1
+ }
+ $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE"
+} else {
+ # use the Python executable known at configure time
+ $_colcon_python_executable="/usr/bin/python3"
+ # if it doesn't exist try a fall back
+ if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) {
+ if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) {
+ echo "error: unable to find python3 executable"
+ exit 1
+ }
+ $_colcon_python_executable="python3"
+ }
+}
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+function _colcon_prefix_powershell_source_script {
+ param (
+ $_colcon_prefix_powershell_source_script_param
+ )
+ # source script with conditional trace output
+ if (Test-Path $_colcon_prefix_powershell_source_script_param) {
+ if ($env:COLCON_TRACE) {
+ echo ". '$_colcon_prefix_powershell_source_script_param'"
+ }
+ . "$_colcon_prefix_powershell_source_script_param"
+ } else {
+ Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'"
+ }
+}
+
+# get all commands in topological order
+$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1
+
+# execute all commands in topological order
+if ($env:COLCON_TRACE) {
+ echo "Execute generated script:"
+ echo "<<<"
+ $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output
+ echo ">>>"
+}
+if ($_colcon_ordered_commands) {
+ $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression
+}
diff --git a/install/local_setup.sh b/install/local_setup.sh
new file mode 100644
index 00000000..6ddd1a7f
--- /dev/null
+++ b/install/local_setup.sh
@@ -0,0 +1,137 @@
+# generated from colcon_core/shell/template/prefix.sh.em
+
+# This script extends the environment with all packages contained in this
+# prefix path.
+
+# since a plain shell script can't determine its own path when being sourced
+# either use the provided COLCON_CURRENT_PREFIX
+# or fall back to the build time prefix (if it exists)
+_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/ws/install"
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then
+ echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
+ unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX
+ return 1
+ fi
+else
+ _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prefix_sh_prepend_unique_value() {
+ # arguments
+ _listname="$1"
+ _value="$2"
+
+ # get values from variable
+ eval _values=\"\$$_listname\"
+ # backup the field separator
+ _colcon_prefix_sh_prepend_unique_value_IFS="$IFS"
+ IFS=":"
+ # start with the new value
+ _all_values="$_value"
+ _contained_value=""
+ # iterate over existing values in the variable
+ for _item in $_values; do
+ # ignore empty strings
+ if [ -z "$_item" ]; then
+ continue
+ fi
+ # ignore duplicates of _value
+ if [ "$_item" = "$_value" ]; then
+ _contained_value=1
+ continue
+ fi
+ # keep non-duplicate values
+ _all_values="$_all_values:$_item"
+ done
+ unset _item
+ if [ -z "$_contained_value" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ if [ "$_all_values" = "$_value" ]; then
+ echo "export $_listname=$_value"
+ else
+ echo "export $_listname=$_value:\$$_listname"
+ fi
+ fi
+ fi
+ unset _contained_value
+ # restore the field separator
+ IFS="$_colcon_prefix_sh_prepend_unique_value_IFS"
+ unset _colcon_prefix_sh_prepend_unique_value_IFS
+ # export the updated variable
+ eval export $_listname=\"$_all_values\"
+ unset _all_values
+ unset _values
+
+ unset _value
+ unset _listname
+}
+
+# add this prefix to the COLCON_PREFIX_PATH
+_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX"
+unset _colcon_prefix_sh_prepend_unique_value
+
+# check environment variable for custom Python executable
+if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then
+ if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then
+ echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist"
+ return 1
+ fi
+ _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE"
+else
+ # try the Python executable known at configure time
+ _colcon_python_executable="/usr/bin/python3"
+ # if it doesn't exist try a fall back
+ if [ ! -f "$_colcon_python_executable" ]; then
+ if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then
+ echo "error: unable to find python3 executable"
+ return 1
+ fi
+ _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"`
+ fi
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_sh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# get all commands in topological order
+_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)"
+unset _colcon_python_executable
+if [ -n "$COLCON_TRACE" ]; then
+ echo "_colcon_prefix_sh_source_script() {
+ if [ -f \"\$1\" ]; then
+ if [ -n \"\$COLCON_TRACE\" ]; then
+ echo \"# . \\\"\$1\\\"\"
+ fi
+ . \"\$1\"
+ else
+ echo \"not found: \\\"\$1\\\"\" 1>&2
+ fi
+ }"
+ echo "# Execute generated script:"
+ echo "# <<<"
+ echo "${_colcon_ordered_commands}"
+ echo "# >>>"
+ echo "unset _colcon_prefix_sh_source_script"
+fi
+eval "${_colcon_ordered_commands}"
+unset _colcon_ordered_commands
+
+unset _colcon_prefix_sh_source_script
+
+unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX
diff --git a/install/local_setup.zsh b/install/local_setup.zsh
new file mode 100644
index 00000000..b6487102
--- /dev/null
+++ b/install/local_setup.zsh
@@ -0,0 +1,134 @@
+# generated from colcon_zsh/shell/template/prefix.zsh.em
+
+# This script extends the environment with all packages contained in this
+# prefix path.
+
+# a zsh script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)"
+else
+ _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to convert array-like strings into arrays
+# to workaround SH_WORD_SPLIT not being set
+_colcon_prefix_zsh_convert_to_array() {
+ local _listname=$1
+ local _dollar="$"
+ local _split="{="
+ local _to_array="(\"$_dollar$_split$_listname}\")"
+ eval $_listname=$_to_array
+}
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prefix_zsh_prepend_unique_value() {
+ # arguments
+ _listname="$1"
+ _value="$2"
+
+ # get values from variable
+ eval _values=\"\$$_listname\"
+ # backup the field separator
+ _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS"
+ IFS=":"
+ # start with the new value
+ _all_values="$_value"
+ _contained_value=""
+ # workaround SH_WORD_SPLIT not being set
+ _colcon_prefix_zsh_convert_to_array _values
+ # iterate over existing values in the variable
+ for _item in $_values; do
+ # ignore empty strings
+ if [ -z "$_item" ]; then
+ continue
+ fi
+ # ignore duplicates of _value
+ if [ "$_item" = "$_value" ]; then
+ _contained_value=1
+ continue
+ fi
+ # keep non-duplicate values
+ _all_values="$_all_values:$_item"
+ done
+ unset _item
+ if [ -z "$_contained_value" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ if [ "$_all_values" = "$_value" ]; then
+ echo "export $_listname=$_value"
+ else
+ echo "export $_listname=$_value:\$$_listname"
+ fi
+ fi
+ fi
+ unset _contained_value
+ # restore the field separator
+ IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS"
+ unset _colcon_prefix_zsh_prepend_unique_value_IFS
+ # export the updated variable
+ eval export $_listname=\"$_all_values\"
+ unset _all_values
+ unset _values
+
+ unset _value
+ unset _listname
+}
+
+# add this prefix to the COLCON_PREFIX_PATH
+_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX"
+unset _colcon_prefix_zsh_prepend_unique_value
+unset _colcon_prefix_zsh_convert_to_array
+
+# check environment variable for custom Python executable
+if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then
+ if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then
+ echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist"
+ return 1
+ fi
+ _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE"
+else
+ # try the Python executable known at configure time
+ _colcon_python_executable="/usr/bin/python3"
+ # if it doesn't exist try a fall back
+ if [ ! -f "$_colcon_python_executable" ]; then
+ if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then
+ echo "error: unable to find python3 executable"
+ return 1
+ fi
+ _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"`
+ fi
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_sh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# get all commands in topological order
+_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)"
+unset _colcon_python_executable
+if [ -n "$COLCON_TRACE" ]; then
+ echo "$(declare -f _colcon_prefix_sh_source_script)"
+ echo "# Execute generated script:"
+ echo "# <<<"
+ echo "${_colcon_ordered_commands}"
+ echo "# >>>"
+ echo "unset _colcon_prefix_sh_source_script"
+fi
+eval "${_colcon_ordered_commands}"
+unset _colcon_ordered_commands
+
+unset _colcon_prefix_sh_source_script
+
+unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX
diff --git a/install/setup.bash b/install/setup.bash
new file mode 100644
index 00000000..10ea0f7c
--- /dev/null
+++ b/install/setup.bash
@@ -0,0 +1,31 @@
+# generated from colcon_bash/shell/template/prefix_chain.bash.em
+
+# This script extends the environment with the environment of other prefix
+# paths which were sourced when this file was generated as well as all packages
+# contained in this prefix path.
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_chain_bash_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# source chained prefixes
+# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
+COLCON_CURRENT_PREFIX="/opt/ros/humble"
+_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
+
+# source this prefix
+# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
+COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)"
+_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
+
+unset COLCON_CURRENT_PREFIX
+unset _colcon_prefix_chain_bash_source_script
diff --git a/install/setup.ps1 b/install/setup.ps1
new file mode 100644
index 00000000..558e9b9e
--- /dev/null
+++ b/install/setup.ps1
@@ -0,0 +1,29 @@
+# generated from colcon_powershell/shell/template/prefix_chain.ps1.em
+
+# This script extends the environment with the environment of other prefix
+# paths which were sourced when this file was generated as well as all packages
+# contained in this prefix path.
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+function _colcon_prefix_chain_powershell_source_script {
+ param (
+ $_colcon_prefix_chain_powershell_source_script_param
+ )
+ # source script with conditional trace output
+ if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) {
+ if ($env:COLCON_TRACE) {
+ echo ". '$_colcon_prefix_chain_powershell_source_script_param'"
+ }
+ . "$_colcon_prefix_chain_powershell_source_script_param"
+ } else {
+ Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'"
+ }
+}
+
+# source chained prefixes
+_colcon_prefix_chain_powershell_source_script "/opt/ros/humble\local_setup.ps1"
+
+# source this prefix
+$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent)
+_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1"
diff --git a/install/setup.sh b/install/setup.sh
new file mode 100644
index 00000000..7a978ade
--- /dev/null
+++ b/install/setup.sh
@@ -0,0 +1,45 @@
+# generated from colcon_core/shell/template/prefix_chain.sh.em
+
+# This script extends the environment with the environment of other prefix
+# paths which were sourced when this file was generated as well as all packages
+# contained in this prefix path.
+
+# since a plain shell script can't determine its own path when being sourced
+# either use the provided COLCON_CURRENT_PREFIX
+# or fall back to the build time prefix (if it exists)
+_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/ws/install
+if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then
+ _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then
+ echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
+ unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX
+ return 1
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_chain_sh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# source chained prefixes
+# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script
+COLCON_CURRENT_PREFIX="/opt/ros/humble"
+_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh"
+
+
+# source this prefix
+# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script
+COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX"
+_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh"
+
+unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX
+unset _colcon_prefix_chain_sh_source_script
+unset COLCON_CURRENT_PREFIX
diff --git a/install/setup.zsh b/install/setup.zsh
new file mode 100644
index 00000000..54799fde
--- /dev/null
+++ b/install/setup.zsh
@@ -0,0 +1,31 @@
+# generated from colcon_zsh/shell/template/prefix_chain.zsh.em
+
+# This script extends the environment with the environment of other prefix
+# paths which were sourced when this file was generated as well as all packages
+# contained in this prefix path.
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_chain_zsh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# source chained prefixes
+# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
+COLCON_CURRENT_PREFIX="/opt/ros/humble"
+_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh"
+
+# source this prefix
+# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
+COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)"
+_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh"
+
+unset COLCON_CURRENT_PREFIX
+unset _colcon_prefix_chain_zsh_source_script
diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE
new file mode 100644
index 00000000..e69de29b
diff --git a/log/latest b/log/latest
new file mode 120000
index 00000000..b57d247c
--- /dev/null
+++ b/log/latest
@@ -0,0 +1 @@
+latest_build
\ No newline at end of file
diff --git a/log/latest_build b/log/latest_build
new file mode 120000
index 00000000..36f1f9ad
--- /dev/null
+++ b/log/latest_build
@@ -0,0 +1 @@
+build_2025-10-23_00-39-04
\ No newline at end of file
diff --git a/opencv_testing/pathfinder.py b/opencv_testing/pathfinder.py
index 22a52d5e..2e9157ea 100644
--- a/opencv_testing/pathfinder.py
+++ b/opencv_testing/pathfinder.py
@@ -3,7 +3,7 @@
speed = 0.0 # mph
max_accel = 3.0 # mph per second
-max_steering = 25 # degrees
+max_steering = 90 # degrees
time_step = 1 # seconds
theta1 = -5
diff --git a/requirements.txt b/requirements.txt
index 9d855a39..353a4f87 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -1,5 +1,5 @@
numpy<2
-opencv-python
+opencv-python<4.10.0
scipy
empy==3.3.4
lark-parser>=0.11
diff --git a/scripts/devcontainer_post_create.sh b/scripts/devcontainer_post_create.sh
index e6fb2cd9..fcbf5838 100644
--- a/scripts/devcontainer_post_create.sh
+++ b/scripts/devcontainer_post_create.sh
@@ -3,6 +3,9 @@ set -euo pipefail
: "${ROS_DISTRO:=humble}"
+apt update
+apt install python3.10-venv
+
if [ -d "/ws/.venv" ]; then
rm -rf /ws/.venv
fi
diff --git a/scripts/run_sim.sh b/scripts/run_sim.sh
new file mode 100644
index 00000000..a9af9037
--- /dev/null
+++ b/scripts/run_sim.sh
@@ -0,0 +1,3 @@
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
+ros2 launch autonomous_kart bringup_sim.launch.py
\ No newline at end of file
diff --git a/scripts/start.bash b/scripts/start.bash
new file mode 100644
index 00000000..caf1d3a9
--- /dev/null
+++ b/scripts/start.bash
@@ -0,0 +1,8 @@
+#!/usr/bin/env bash
+
+source /opt/ros/humble/setup.bash
+[ -f /ws/install/setup.bash ] && source /ws/install/setup.bash
+[ -f /ws/.venv/bin/activate ] && source /ws/.venv/bin/activate
+
+colcon build
+ros2 launch autonomous_kart bringup_sim.launch.py
\ No newline at end of file
diff --git a/src/autonomous_kart/autonomous_kart/launch/bringup_sim.launch.py b/src/autonomous_kart/autonomous_kart/launch/bringup_sim.launch.py
index 3e8f0212..12db782c 100644
--- a/src/autonomous_kart/autonomous_kart/launch/bringup_sim.launch.py
+++ b/src/autonomous_kart/autonomous_kart/launch/bringup_sim.launch.py
@@ -12,34 +12,35 @@ def generate_launch_description():
package='autonomous_kart',
executable='motor_node',
name='motor_node',
- parameters=[os.path.join(pkg_share, 'params', 'controller.yaml'), {'simulation_mode': True}]
+ parameters=[os.path.join(pkg_share, 'params', 'controller.yaml'),
+ os.path.join(pkg_share, 'params', 'system.yaml'), {'simulation_mode': True}]
),
Node(
package='autonomous_kart',
executable='steering_node',
name='steering_node',
- parameters=[os.path.join(pkg_share, 'params', 'controller.yaml'), {'simulation_mode': True}]
+ parameters=[os.path.join(pkg_share, 'params', 'controller.yaml'),
+ os.path.join(pkg_share, 'params', 'system.yaml'), {'simulation_mode': True}]
),
Node(
package='autonomous_kart',
executable='camera_node',
name='camera_node',
- parameters=[os.path.join(pkg_share, 'params', 'camera.yaml'), {'simulation_mode': True}]
+ parameters=[os.path.join(pkg_share, 'params', 'camera.yaml'),
+ os.path.join(pkg_share, 'params', 'system.yaml'), {'simulation_mode': True}]
),
Node(
package='autonomous_kart',
executable='pathfinder_node',
name='pathfinder_node',
- parameters=[os.path.join(pkg_share, 'params', 'planner.yaml'),
- os.path.join(pkg_share, 'params', 'safety.yaml'), os.path.join(pkg_share, 'params', 'gps.yaml'),
- {'simulation_mode': True}]
+ parameters=[os.path.join(pkg_share, 'params', 'gps.yaml'), os.path.join(pkg_share, 'params', 'system.yaml'),
+ os.path.join(pkg_share, 'params', 'safety.yaml'), {'simulation_mode': True}]
),
Node(
package='autonomous_kart',
executable='opencv_pathfinder_node',
name='opencv_pathfinder_node',
- parameters=[os.path.join(pkg_share, 'params', 'planner.yaml'),
- os.path.join(pkg_share, 'params', 'safety.yaml'), os.path.join(pkg_share, 'params', 'gps.yaml'),
+ parameters=[os.path.join(pkg_share, 'params', 'gps.yaml'), os.path.join(pkg_share, 'params', 'system.yaml'),
{'simulation_mode': True}]
),
])
diff --git a/src/autonomous_kart/autonomous_kart/nodes/camera/camera_node.py b/src/autonomous_kart/autonomous_kart/nodes/camera/camera_node.py
index d1e48c39..19101224 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/camera/camera_node.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/camera/camera_node.py
@@ -13,7 +13,7 @@
class CameraNode(Node):
def __init__(self):
super().__init__("camera_node")
- self.last_callback_time = None
+ self.last_callback_time = time.time()
self.logger = self.get_logger()
self.declare_parameter("simulation_mode", True)
@@ -22,7 +22,8 @@ def __init__(self):
self.frame_counter = 0
if self.fps == 0: # div by 0 error later
- self.fps = 60
+ self.declare_parameter("system_frequency", 60.0)
+ self.fps = self.get_parameter("system_frequency").value
self.sim_mode = self.get_parameter("simulation_mode").value
@@ -66,14 +67,9 @@ def timer_callback(self):
"""
Publishes the next frame
"""
- if not hasattr(self, 'last_callback_time'):
- self.last_callback_time = time.time()
-
if self.sim_mode:
with self.frame_lock:
if self.latest_frame is not None:
- pub_start = time.time()
-
self.image_pub.publish(self.latest_frame)
self.frame_counter += 1
else:
@@ -108,7 +104,7 @@ def read_frames(self):
ret, frame = self.cap.read()
else:
height, width = frame.shape[:2]
- target_width = 360
+ target_width = 360 # 360x202 BGR image optimized for jetson communication
target_height = int(height * (target_width / width))
resized = cv2.resize(frame, (target_width, target_height))
msg = self.bridge.cv2_to_imgmsg(resized, "bgr8")
diff --git a/src/autonomous_kart/autonomous_kart/nodes/motor/motor_node.py b/src/autonomous_kart/autonomous_kart/nodes/motor/motor_node.py
index c90c08b8..cec6597e 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/motor/motor_node.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/motor/motor_node.py
@@ -21,14 +21,14 @@ def __init__(self):
Float32,
'cmd_vel',
self.cmd_vel_callback,
- 1
+ 5
)
# Publisher for motor speed
self.speed_pub = self.create_publisher(
Float32,
'motor_speed',
- 1
+ 5
)
self.current_speed = Float32()
diff --git a/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/opencv_pathfinder_node.py b/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/opencv_pathfinder_node.py
index e0fa3bc2..97b4edcf 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/opencv_pathfinder_node.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/opencv_pathfinder_node.py
@@ -21,13 +21,16 @@ def __init__(self):
self.logger = self.get_logger()
self.bridge = CvBridge()
self.frame_count = 0
- self.total_time = 0
+ self.angle_msg = None
+
+ self.declare_parameter('system_frequency', 60)
+ self.system_frequency = self.get_parameter('system_frequency').value
qos = QoSProfile(
depth=1,
reliability=ReliabilityPolicy.BEST_EFFORT,
durability=DurabilityPolicy.VOLATILE,
- lifespan=Duration(seconds=0, nanoseconds=int(1e9 / 60)) # TODO: Make this not hardcoded
+ lifespan=Duration(seconds=0, nanoseconds=int(1e9 / self.system_frequency))
)
# Subscribe to camera
@@ -42,13 +45,13 @@ def __init__(self):
self.angle_pub = self.create_publisher(
Float32MultiArray,
'track_angles',
- 1,
+ 5,
)
self.logger.info("Pathfinder Node started - subscribed to /camera/image_raw")
def image_callback(self, msg):
- frame = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+ frame = self.bridge.imgmsg_to_cv2(msg, "passthrough")
self.frame_count += 1
self.frames_since_last_log += 1
@@ -66,7 +69,12 @@ def image_callback(self, msg):
# Publish angles
angles = calculate_track_angles(frame)
- self.angle_pub.publish(Float32MultiArray(data=angles))
+ if not self.angle_msg:
+ self.angle_msg = Float32MultiArray(data=angles)
+ else:
+ # self.angle_pub.publish(Float32MultiArray(data=angles))
+ self.angle_msg.data = angles
+ self.angle_pub.publish(self.angle_msg)
def main(args=None):
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
index 5ee078f0..de813414 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
@@ -2,7 +2,39 @@
from typing import Tuple
-def pathfinder(opencv_output: Tuple):
+def pathfinder(opencv_output: Tuple, logger):
+
+ speed = 0.0 # mph
+ max_accel = 3.0 # mph per second
+ max_steering = 25 # degrees
+ time_step = 1 # seconds
+
+ theta1 = opencv_output[0]
+ theta2 = opencv_output[1]
+
+ desired_heading = (theta1 + theta2) / 2 #average of both angles
+
+ if abs(desired_heading) < 10:
+ target_speed = 30 #max speed on straights
+ else:
+ target_speed = 15 #target speed on turns
+
+ if speed < target_speed:
+ speed += max_accel * time_step #physics c: mechanics
+ if speed > target_speed:
+ speed = target_speed
+ else:
+ speed -= max_accel * time_step
+ if speed < target_speed:
+ speed = target_speed
+
+ steering_command = max(min(desired_heading, max_steering), -max_steering)
+
+ get_logger.info(f"Theta1: {theta1:.1f}°, Theta2: {theta2:.1f}°")
+ get_logger.info(f"Desired Heading: {desired_heading:.1f}°")
+ get_logger.info(f"Target Speed: {target_speed} mph | Current Speed: {speed:.1f} mph")
+ get_logger.info(f"Steering Command: {steering_command:.1f}° {'Left' if steering_command < 0 else 'Right' if steering_command > 0 else 'Straight'}")
+
"""
Calculate commands for steering and motor from opencv_pathfinder efficiently
Part of hot loop so must be efficient.
@@ -11,8 +43,8 @@ def pathfinder(opencv_output: Tuple):
:return: Returns commands to motor & steering in (speed % of total, steering angle (degrees from -90 to 90 with 0 as straight)
"""
- # Dummy
- motor_speed = 20 * random.random()
- steering_angle = 180 * random.random() - 90
+ Dummy
+ motor_speed = 7 #20 * random.random()
+ steering_angle = 7 #180 * random.random() - 90
- return motor_speed, steering_angle
\ No newline at end of file
+ return motor_speed, steering_angle
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
index c5045998..cb11b8fa 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
@@ -1,10 +1,6 @@
-import random
-import time
-
import rclpy
from rclpy.node import Node
-from rclpy.executors import MultiThreadedExecutor
-from std_msgs.msg import Float64MultiArray, Float32, Float32MultiArray
+from std_msgs.msg import Float32MultiArray, Float32
from autonomous_kart.nodes.pathfinder.pathfinder import pathfinder
@@ -18,44 +14,47 @@ def __init__(self):
self.cmd_count = 0
self.last_log_time = self.get_clock().now()
+ self.declare_parameter('system_frequency', 60)
+ self.system_frequency = self.get_parameter('system_frequency').value
+
# Timer to log average every 5 seconds
self.create_timer(5.0, self.log_command_rate)
# Subscriber to opencv pathfinder for angles
self.opencv_pathfinder_subscriber = self.create_subscription(
- Float64MultiArray,
+ Float32MultiArray,
'track_angles',
self.calculate_path_callback,
- 1
+ 5
)
# Publisher to motor
self.motor_publisher = self.create_publisher(
Float32,
'cmd_vel',
- 1
+ 5
)
# # Publisher to steering
self.steering_publisher = self.create_publisher(
Float32,
'cmd_turn',
- 1
+ 5
)
self.logger.info("Initialize Pathfinder Node")
- def calculate_path_callback(self, msg: Float64MultiArray):
+ def calculate_path_callback(self, msg: Float32MultiArray):
"""
Calculate commands for steering and motor from opencv_pathfinder efficiently
Part of hot loop so must be efficient
- :param msg: Float64MultiArray of [left angle from center to base of track from image, right angle ...]
+ :param msg: Float32MultiArray of [left angle from center to base of track from image, right angle ...]
:return: Publishes commands to motor & steering
"""
self.cmd_count += 1
self.angles = (msg.data[0], msg.data[1])
- motor_speed, steering_angle = pathfinder(msg.data)
+ motor_speed, steering_angle = pathfinder(msg.data, self.logger)
self.steering_publisher.publish(Float32(data=steering_angle))
self.motor_publisher.publish(Float32(data=motor_speed))
@@ -79,19 +78,14 @@ def log_command_rate(self):
def main(args=None):
rclpy.init(args=args)
node = PathfinderNode()
-
- executor = MultiThreadedExecutor(num_threads=2)
- executor.add_node(node)
try:
- executor.spin()
+ rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
- node.running = False
- time.sleep(0.1)
node.destroy_node()
- executor.shutdown()
+ rclpy.shutdown()
if __name__ == '__main__':
- main()
+ main()
\ No newline at end of file
diff --git a/src/autonomous_kart/autonomous_kart/nodes/steering/steering_node.py b/src/autonomous_kart/autonomous_kart/nodes/steering/steering_node.py
index c086be81..1fa85207 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/steering/steering_node.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/steering/steering_node.py
@@ -21,14 +21,14 @@ def __init__(self):
Float32,
'cmd_turn',
self.cmd_turn_callback,
- 1
+ 5
)
# Publisher for steering angular velocity
self.turn_pub = self.create_publisher(
Float32,
'turn_angle',
- 1
+ 5
)
self.current_angle = Float32()
diff --git a/src/autonomous_kart/autonomous_kart/params/camera.yaml b/src/autonomous_kart/autonomous_kart/params/camera.yaml
index 346227a4..a4cbf016 100644
--- a/src/autonomous_kart/autonomous_kart/params/camera.yaml
+++ b/src/autonomous_kart/autonomous_kart/params/camera.yaml
@@ -2,4 +2,4 @@
camera_node:
ros__parameters:
simulation_mode: false # set later
- fps: 30.0 # float
\ No newline at end of file
+ fps: 60.0 # float
\ No newline at end of file
diff --git a/src/autonomous_kart/autonomous_kart/params/system.yaml b/src/autonomous_kart/autonomous_kart/params/system.yaml
new file mode 100644
index 00000000..8161fb78
--- /dev/null
+++ b/src/autonomous_kart/autonomous_kart/params/system.yaml
@@ -0,0 +1,4 @@
+system_config:
+ ros__parameters:
+ simulation_mode: false,
+ system_frequency: 60 # Speed of hot loop (Hertz)
\ No newline at end of file
From a17a902f5c4c28c59cd1659f755bd4991d69e20a Mon Sep 17 00:00:00 2001
From: Hirthik Gopal Shanmugam <141051449+hgs2007@users.noreply.github.com>
Date: Tue, 30 Sep 2025 20:00:34 -0400
Subject: [PATCH 05/21] black and white video
testing
---
opencv_testing/opencv_testing.py | 19 +++++++++++++++++++
1 file changed, 19 insertions(+)
create mode 100644 opencv_testing/opencv_testing.py
diff --git a/opencv_testing/opencv_testing.py b/opencv_testing/opencv_testing.py
new file mode 100644
index 00000000..8edac464
--- /dev/null
+++ b/opencv_testing/opencv_testing.py
@@ -0,0 +1,19 @@
+import cv2
+
+cap = cv2.VideoCapture("IMG_8824.mp4") # or 0 for webcam
+
+while True:
+ ret, frame = cap.read()
+ if not ret:
+ break # end of video
+
+ gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+
+ cv2.imshow("Video", gray_frame) # show the video in a window
+
+ # Press 'q' to exit early
+ if cv2.waitKey(25) & 0xFF == ord('q'):
+ break
+
+cap.release()
+cv2.destroyAllWindows()
From cdb613537274bec0b90c69b05d1cec6f9d223068 Mon Sep 17 00:00:00 2001
From: Hirthik Gopal Shanmugam <141051449+hgs2007@users.noreply.github.com>
Date: Tue, 21 Oct 2025 18:24:01 -0400
Subject: [PATCH 06/21] calculates speed, steering input, and acceleration
using angles
---
opencv_testing/opencv_testing.py | 6 ++--
opencv_testing/pathfinder.py | 50 ++++++++++++++++++++++++++++++++
2 files changed, 53 insertions(+), 3 deletions(-)
create mode 100644 opencv_testing/pathfinder.py
diff --git a/opencv_testing/opencv_testing.py b/opencv_testing/opencv_testing.py
index 8edac464..dc56c1f4 100644
--- a/opencv_testing/opencv_testing.py
+++ b/opencv_testing/opencv_testing.py
@@ -1,15 +1,15 @@
import cv2
-cap = cv2.VideoCapture("IMG_8824.mp4") # or 0 for webcam
+cap = cv2.VideoCapture("IMG_8824.mp4")
while True:
ret, frame = cap.read()
if not ret:
- break # end of video
+ break
gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
- cv2.imshow("Video", gray_frame) # show the video in a window
+ cv2.imshow("Video", gray_frame) #shows the video
# Press 'q' to exit early
if cv2.waitKey(25) & 0xFF == ord('q'):
diff --git a/opencv_testing/pathfinder.py b/opencv_testing/pathfinder.py
new file mode 100644
index 00000000..22a52d5e
--- /dev/null
+++ b/opencv_testing/pathfinder.py
@@ -0,0 +1,50 @@
+import time
+import os
+
+speed = 0.0 # mph
+max_accel = 3.0 # mph per second
+max_steering = 25 # degrees
+time_step = 1 # seconds
+
+theta1 = -5
+theta2 = 5
+
+for t in range(1, 31):
+ if t <= 10:
+ theta1 = -5
+ theta2 = 5
+ elif 10 < t <= 20:
+ theta1 -= 2
+ theta2 += 0.5
+ elif 20 < t <= 30:
+ theta1 += 2
+ theta2 += 2
+
+ desired_heading = (theta1 + theta2) / 2 #average of both angles
+
+ if abs(desired_heading) < 10:
+ target_speed = 30 #max speed on straights
+ else:
+ target_speed = 15 #target speed on turns
+
+ if speed < target_speed:
+ speed += max_accel * time_step #physics c: mechanics
+ if speed > target_speed:
+ speed = target_speed
+ else:
+ speed -= max_accel * time_step
+ if speed < target_speed:
+ speed = target_speed
+
+ steering_command = max(min(desired_heading, max_steering), -max_steering)
+
+ os.system('cls' if os.name == 'nt' else 'clear')
+
+ print(f"Time: {t} sec")
+ print(f"Theta1: {theta1:.1f}°, Theta2: {theta2:.1f}°")
+ print(f"Desired Heading: {desired_heading:.1f}°")
+ print(f"Target Speed: {target_speed} mph | Current Speed: {speed:.1f} mph")
+ print(f"Steering Command: {steering_command:.1f}° {'Left' if steering_command < 0 else 'Right' if steering_command > 0 else 'Straight'}")
+
+ time.sleep(1)
+
From 45fb819111feb4d9b3f9187f8d876402b3d82640 Mon Sep 17 00:00:00 2001
From: Hirthik Gopal Shanmugam <141051449+hgs2007@users.noreply.github.com>
Date: Tue, 21 Oct 2025 18:32:54 -0400
Subject: [PATCH 07/21] calculates angles
---
opencv_testing/angle_calculator.py | 184 +++++++++++++++++++++++++++++
1 file changed, 184 insertions(+)
create mode 100644 opencv_testing/angle_calculator.py
diff --git a/opencv_testing/angle_calculator.py b/opencv_testing/angle_calculator.py
new file mode 100644
index 00000000..54488c3a
--- /dev/null
+++ b/opencv_testing/angle_calculator.py
@@ -0,0 +1,184 @@
+"""
+Compute theta_left and theta_right from a video.
+Requirements: opencv-python (cv2), numpy
+Usage: tweak CAMERA_FX or HFOV_HORIZONTAL to match your camera.
+"""
+
+import cv2
+import numpy as np
+import math
+
+# ----------------- CONFIG -----------------
+VIDEO_PATH = "IMG_8824.mp4" # change to your file or use 0 for webcam
+USE_CALIBRATION = False # True if you have fx, cx from calibration
+CAMERA_FX = 800.0 # focal length in pixels (only if USE_CALIBRATION)
+CAMERA_CX = None # principal point x; if None -> image_width/2
+HFOV_DEG = 70.0 # horizontal field of view (deg) if no fx available
+
+SMOOTH_ALPHA = 0.7 # for low-pass filtering of theta
+CANNY_THRESH1 = 50
+CANNY_THRESH2 = 150
+
+# Ray sampling parameters
+MAX_SAMPLE_DIST = 400 # max pixels to scan along each ray
+FORWARD_STEP = 1 # pixels per sample along forward ray
+RIGHT_STEP = 1 # pixels per sample along right ray
+
+# -----------------------------------------
+
+def compute_fx_from_hfov(width, hfov_deg):
+ hfov = math.radians(hfov_deg)
+ return (width / 2.0) / math.tan(hfov / 2.0)
+
+def pixel_to_angle(u, fx, cx):
+ # returns angle in degrees, negative = left of center
+ return math.degrees(math.atan2((u - cx), fx))
+
+def find_intersection_along_column(mask, col, start_row, step=1, max_dist=400):
+ """
+ Scan downwards along column 'col' starting at start_row (row index),
+ return (u,v) of first mask nonzero pixel, or None if none found.
+ """
+ h, w = mask.shape
+ row = start_row
+ dist = 0
+ while dist < max_dist and 0 <= row < h:
+ if mask[row, col]:
+ return col, row
+ row += step
+ dist += abs(step)
+ return None
+
+def find_intersection_along_row(mask, row, start_col, step=1, max_dist=400):
+ """
+ Scan rightwards along row 'row' starting at start_col, return first mask hit.
+ """
+ h, w = mask.shape
+ col = start_col
+ dist = 0
+ while dist < max_dist and 0 <= col < w:
+ if mask[row, col]:
+ return col, row
+ col += step
+ dist += abs(step)
+ return None
+
+def main():
+ cap = cv2.VideoCapture(VIDEO_PATH)
+ if not cap.isOpened():
+ print("Cannot open video:", VIDEO_PATH)
+ return
+
+ theta_left_f = None
+ theta_right_f = None
+
+ while True:
+ ret, frame = cap.read()
+ if not ret:
+ break
+
+ h, w = frame.shape[:2]
+ cx = CAMERA_CX if CAMERA_CX is not None else w / 2.0
+ if USE_CALIBRATION:
+ fx = CAMERA_FX
+ else:
+ fx = compute_fx_from_hfov(w, HFOV_DEG)
+
+ # 1) preprocess and boundary mask
+ gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+ blur = cv2.GaussianBlur(gray, (5,5), 0)
+ edges = cv2.Canny(blur, CANNY_THRESH1, CANNY_THRESH2)
+ # optional morphology to fill gaps
+ kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5,5))
+ mask = cv2.morphologyEx(edges, cv2.MORPH_CLOSE, kernel)
+
+ # 2) define rays: use image center
+ center_col = int(round(cx))
+ center_row = int(round(h / 2))
+
+ # forward ray: downwards from center_row toward bottom (increasing row)
+ f_hit = find_intersection_along_column(mask, center_col, center_row, step=FORWARD_STEP, max_dist=MAX_SAMPLE_DIST)
+
+ # right ray: from center out to the right along center_row
+ r_hit = find_intersection_along_row(mask, center_row, center_col, step=RIGHT_STEP, max_dist=MAX_SAMPLE_DIST)
+
+ # convert hits to angles (default to None/confidence if not found)
+ theta_left = None
+ theta_right = None
+
+ if f_hit is not None:
+ u_f, v_f = f_hit
+ theta_forward = pixel_to_angle(u_f, fx, cx) # bearing of forward intersection
+ # If the forward ray hits boundary on right side of center, we can treat that as right boundary?
+ # However your definition: theta_left is angle from left boundary to center — we approximate:
+ # We'll set theta_left to the bearing of the left-side boundary detected forward of center,
+ # but since forward ray is center column, use it as a nearby boundary reading if needed.
+ # For consistent approach: treat forward ray hit on its x location to compute whichever boundary that represents.
+ if u_f < cx:
+ theta_left = pixel_to_angle(u_f, fx, cx)
+ else:
+ theta_right = pixel_to_angle(u_f, fx, cx)
+
+ if r_hit is not None:
+ u_r, v_r = r_hit
+ # point to the right of center -> this is likely the right boundary
+ theta_right = pixel_to_angle(u_r, fx, cx)
+
+ # If we failed to find one of them from the rays, try alternate strategy:
+ # find contours and pick nearest contour point to the ray direction (omitted for brevity),
+ # or fallback to last frame value.
+
+ # fallback: if missing, reuse previous smoothed value or compute from any contour
+ if theta_left is None and theta_left_f is not None:
+ theta_left = theta_left_f
+ if theta_right is None and theta_right_f is not None:
+ theta_right = theta_right_f
+
+ # smoothing
+ if theta_left is not None:
+ theta_left_f = theta_left if theta_left_f is None else (SMOOTH_ALPHA * theta_left_f + (1-SMOOTH_ALPHA)*theta_left)
+ if theta_right is not None:
+ theta_right_f = theta_right if theta_right_f is None else (SMOOTH_ALPHA * theta_right_f + (1-SMOOTH_ALPHA)*theta_right)
+
+ # compute desired heading if both exist (or using whichever exists)
+ desired_heading = None
+ if (theta_left_f is not None) and (theta_right_f is not None):
+ desired_heading = 0.5 * (theta_left_f + theta_right_f)
+ elif theta_left_f is not None:
+ desired_heading = theta_left_f # single-side fallback
+ elif theta_right_f is not None:
+ desired_heading = theta_right_f
+
+ # Display overlay for debugging
+ vis = frame.copy()
+ # draw rays
+ cv2.line(vis, (center_col, center_row), (center_col, min(h, center_row + MAX_SAMPLE_DIST)), (0,255,0), 1)
+ cv2.line(vis, (center_col, center_row), (min(w-1, center_col + MAX_SAMPLE_DIST), center_row), (0,255,0), 1)
+ if f_hit is not None:
+ cv2.circle(vis, (f_hit[0], f_hit[1]), 6, (0,0,255), -1)
+ if r_hit is not None:
+ cv2.circle(vis, (r_hit[0], r_hit[1]), 6, (255,0,0), -1)
+
+ # text
+ info = [
+ f"theta_left_f: {theta_left_f:.2f}" if theta_left_f is not None else "theta_left_f: N/A",
+ f"theta_right_f: {theta_right_f:.2f}" if theta_right_f is not None else "theta_right_f: N/A",
+ f"desired_heading: {desired_heading:.2f}" if desired_heading is not None else "desired_heading: N/A"
+ ]
+ y = 30
+ for line in info:
+ cv2.putText(vis, line, (10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,255), 2)
+ y += 25
+
+ cv2.imshow("vis", vis)
+ cv2.imshow("mask", mask)
+
+ # press q to quit
+ if cv2.waitKey(1) & 0xFF == ord('q'):
+ break
+
+ cap.release()
+ cv2.destroyAllWindows()
+
+if __name__ == "__main__":
+ main()
From f2133522d234f8ed32210c7610ffd43490d7ebe1 Mon Sep 17 00:00:00 2001
From: Hirthik Gopal Shanmugam <141051449+hgs2007@users.noreply.github.com>
Date: Sun, 2 Nov 2025 16:57:07 -0500
Subject: [PATCH 08/21] pathfinder file
---
install/.colcon_install_layout | 1 +
install/COLCON_IGNORE | 0
install/_local_setup_util_ps1.py | 407 ++++++++++++++++++
install/_local_setup_util_sh.py | 407 ++++++++++++++++++
.../resource_index/packages/autonomous_kart | 0
.../hook/ament_prefix_path.dsv | 1 +
.../hook/ament_prefix_path.ps1 | 3 +
.../autonomous_kart/hook/ament_prefix_path.sh | 3 +
.../share/autonomous_kart/hook/pythonpath.dsv | 1 +
.../share/autonomous_kart/hook/pythonpath.ps1 | 3 +
.../share/autonomous_kart/hook/pythonpath.sh | 3 +
.../launch/bringup_pi.launch.py | 49 +++
.../launch/bringup_sim.launch.py | 45 ++
.../share/autonomous_kart/package.bash | 31 ++
.../share/autonomous_kart/package.dsv | 6 +
.../share/autonomous_kart/package.ps1 | 116 +++++
.../share/autonomous_kart/package.sh | 87 ++++
.../share/autonomous_kart/package.xml | 23 +
.../share/autonomous_kart/package.zsh | 42 ++
.../share/autonomous_kart/params/camera.yaml | 5 +
.../autonomous_kart/params/controller.yaml | 8 +
.../share/autonomous_kart/params/gps.yaml | 0
.../share/autonomous_kart/params/planner.yaml | 1 +
.../share/autonomous_kart/params/safety.yaml | 1 +
.../colcon-core/packages/autonomous_kart | 1 +
install/local_setup.bash | 121 ++++++
install/local_setup.ps1 | 55 +++
install/local_setup.sh | 137 ++++++
install/local_setup.zsh | 134 ++++++
install/setup.bash | 31 ++
install/setup.ps1 | 29 ++
install/setup.sh | 45 ++
install/setup.zsh | 31 ++
log/COLCON_IGNORE | 0
log/latest | 1 +
log/latest_build | 1 +
opencv_testing/pathfinder.py | 2 +-
.../nodes/pathfinder/pathfinder.py | 42 +-
.../nodes/pathfinder/pathfinder_node.py | 19 +-
39 files changed, 1881 insertions(+), 11 deletions(-)
create mode 100644 install/.colcon_install_layout
create mode 100644 install/COLCON_IGNORE
create mode 100644 install/_local_setup_util_ps1.py
create mode 100644 install/_local_setup_util_sh.py
create mode 100644 install/autonomous_kart/share/ament_index/resource_index/packages/autonomous_kart
create mode 100644 install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.dsv
create mode 100644 install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.ps1
create mode 100644 install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.sh
create mode 100644 install/autonomous_kart/share/autonomous_kart/hook/pythonpath.dsv
create mode 100644 install/autonomous_kart/share/autonomous_kart/hook/pythonpath.ps1
create mode 100644 install/autonomous_kart/share/autonomous_kart/hook/pythonpath.sh
create mode 100644 install/autonomous_kart/share/autonomous_kart/launch/bringup_pi.launch.py
create mode 100644 install/autonomous_kart/share/autonomous_kart/launch/bringup_sim.launch.py
create mode 100644 install/autonomous_kart/share/autonomous_kart/package.bash
create mode 100644 install/autonomous_kart/share/autonomous_kart/package.dsv
create mode 100644 install/autonomous_kart/share/autonomous_kart/package.ps1
create mode 100644 install/autonomous_kart/share/autonomous_kart/package.sh
create mode 100644 install/autonomous_kart/share/autonomous_kart/package.xml
create mode 100644 install/autonomous_kart/share/autonomous_kart/package.zsh
create mode 100644 install/autonomous_kart/share/autonomous_kart/params/camera.yaml
create mode 100644 install/autonomous_kart/share/autonomous_kart/params/controller.yaml
create mode 100644 install/autonomous_kart/share/autonomous_kart/params/gps.yaml
create mode 100644 install/autonomous_kart/share/autonomous_kart/params/planner.yaml
create mode 100644 install/autonomous_kart/share/autonomous_kart/params/safety.yaml
create mode 100644 install/autonomous_kart/share/colcon-core/packages/autonomous_kart
create mode 100644 install/local_setup.bash
create mode 100644 install/local_setup.ps1
create mode 100644 install/local_setup.sh
create mode 100644 install/local_setup.zsh
create mode 100644 install/setup.bash
create mode 100644 install/setup.ps1
create mode 100644 install/setup.sh
create mode 100644 install/setup.zsh
create mode 100644 log/COLCON_IGNORE
create mode 120000 log/latest
create mode 120000 log/latest_build
diff --git a/install/.colcon_install_layout b/install/.colcon_install_layout
new file mode 100644
index 00000000..3aad5336
--- /dev/null
+++ b/install/.colcon_install_layout
@@ -0,0 +1 @@
+isolated
diff --git a/install/COLCON_IGNORE b/install/COLCON_IGNORE
new file mode 100644
index 00000000..e69de29b
diff --git a/install/_local_setup_util_ps1.py b/install/_local_setup_util_ps1.py
new file mode 100644
index 00000000..3c6d9e87
--- /dev/null
+++ b/install/_local_setup_util_ps1.py
@@ -0,0 +1,407 @@
+# Copyright 2016-2019 Dirk Thomas
+# Licensed under the Apache License, Version 2.0
+
+import argparse
+from collections import OrderedDict
+import os
+from pathlib import Path
+import sys
+
+
+FORMAT_STR_COMMENT_LINE = '# {comment}'
+FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"'
+FORMAT_STR_USE_ENV_VAR = '$env:{name}'
+FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' # noqa: E501
+FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' # noqa: E501
+FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' # noqa: E501
+
+DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate'
+DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate'
+DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists'
+DSV_TYPE_SET = 'set'
+DSV_TYPE_SET_IF_UNSET = 'set-if-unset'
+DSV_TYPE_SOURCE = 'source'
+
+
+def main(argv=sys.argv[1:]): # noqa: D103
+ parser = argparse.ArgumentParser(
+ description='Output shell commands for the packages in topological '
+ 'order')
+ parser.add_argument(
+ 'primary_extension',
+ help='The file extension of the primary shell')
+ parser.add_argument(
+ 'additional_extension', nargs='?',
+ help='The additional file extension to be considered')
+ parser.add_argument(
+ '--merged-install', action='store_true',
+ help='All install prefixes are merged into a single location')
+ args = parser.parse_args(argv)
+
+ packages = get_packages(Path(__file__).parent, args.merged_install)
+
+ ordered_packages = order_packages(packages)
+ for pkg_name in ordered_packages:
+ if _include_comments():
+ print(
+ FORMAT_STR_COMMENT_LINE.format_map(
+ {'comment': 'Package: ' + pkg_name}))
+ prefix = os.path.abspath(os.path.dirname(__file__))
+ if not args.merged_install:
+ prefix = os.path.join(prefix, pkg_name)
+ for line in get_commands(
+ pkg_name, prefix, args.primary_extension,
+ args.additional_extension
+ ):
+ print(line)
+
+ for line in _remove_ending_separators():
+ print(line)
+
+
+def get_packages(prefix_path, merged_install):
+ """
+ Find packages based on colcon-specific files created during installation.
+
+ :param Path prefix_path: The install prefix path of all packages
+ :param bool merged_install: The flag if the packages are all installed
+ directly in the prefix or if each package is installed in a subdirectory
+ named after the package
+ :returns: A mapping from the package name to the set of runtime
+ dependencies
+ :rtype: dict
+ """
+ packages = {}
+ # since importing colcon_core isn't feasible here the following constant
+ # must match colcon_core.location.get_relative_package_index_path()
+ subdirectory = 'share/colcon-core/packages'
+ if merged_install:
+ # return if workspace is empty
+ if not (prefix_path / subdirectory).is_dir():
+ return packages
+ # find all files in the subdirectory
+ for p in (prefix_path / subdirectory).iterdir():
+ if not p.is_file():
+ continue
+ if p.name.startswith('.'):
+ continue
+ add_package_runtime_dependencies(p, packages)
+ else:
+ # for each subdirectory look for the package specific file
+ for p in prefix_path.iterdir():
+ if not p.is_dir():
+ continue
+ if p.name.startswith('.'):
+ continue
+ p = p / subdirectory / p.name
+ if p.is_file():
+ add_package_runtime_dependencies(p, packages)
+
+ # remove unknown dependencies
+ pkg_names = set(packages.keys())
+ for k in packages.keys():
+ packages[k] = {d for d in packages[k] if d in pkg_names}
+
+ return packages
+
+
+def add_package_runtime_dependencies(path, packages):
+ """
+ Check the path and if it exists extract the packages runtime dependencies.
+
+ :param Path path: The resource file containing the runtime dependencies
+ :param dict packages: A mapping from package names to the sets of runtime
+ dependencies to add to
+ """
+ content = path.read_text()
+ dependencies = set(content.split(os.pathsep) if content else [])
+ packages[path.name] = dependencies
+
+
+def order_packages(packages):
+ """
+ Order packages topologically.
+
+ :param dict packages: A mapping from package name to the set of runtime
+ dependencies
+ :returns: The package names
+ :rtype: list
+ """
+ # select packages with no dependencies in alphabetical order
+ to_be_ordered = list(packages.keys())
+ ordered = []
+ while to_be_ordered:
+ pkg_names_without_deps = [
+ name for name in to_be_ordered if not packages[name]]
+ if not pkg_names_without_deps:
+ reduce_cycle_set(packages)
+ raise RuntimeError(
+ 'Circular dependency between: ' + ', '.join(sorted(packages)))
+ pkg_names_without_deps.sort()
+ pkg_name = pkg_names_without_deps[0]
+ to_be_ordered.remove(pkg_name)
+ ordered.append(pkg_name)
+ # remove item from dependency lists
+ for k in list(packages.keys()):
+ if pkg_name in packages[k]:
+ packages[k].remove(pkg_name)
+ return ordered
+
+
+def reduce_cycle_set(packages):
+ """
+ Reduce the set of packages to the ones part of the circular dependency.
+
+ :param dict packages: A mapping from package name to the set of runtime
+ dependencies which is modified in place
+ """
+ last_depended = None
+ while len(packages) > 0:
+ # get all remaining dependencies
+ depended = set()
+ for pkg_name, dependencies in packages.items():
+ depended = depended.union(dependencies)
+ # remove all packages which are not dependent on
+ for name in list(packages.keys()):
+ if name not in depended:
+ del packages[name]
+ if last_depended:
+ # if remaining packages haven't changed return them
+ if last_depended == depended:
+ return packages.keys()
+ # otherwise reduce again
+ last_depended = depended
+
+
+def _include_comments():
+ # skipping comment lines when COLCON_TRACE is not set speeds up the
+ # processing especially on Windows
+ return bool(os.environ.get('COLCON_TRACE'))
+
+
+def get_commands(pkg_name, prefix, primary_extension, additional_extension):
+ commands = []
+ package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv')
+ if os.path.exists(package_dsv_path):
+ commands += process_dsv_file(
+ package_dsv_path, prefix, primary_extension, additional_extension)
+ return commands
+
+
+def process_dsv_file(
+ dsv_path, prefix, primary_extension=None, additional_extension=None
+):
+ commands = []
+ if _include_comments():
+ commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path}))
+ with open(dsv_path, 'r') as h:
+ content = h.read()
+ lines = content.splitlines()
+
+ basenames = OrderedDict()
+ for i, line in enumerate(lines):
+ # skip over empty or whitespace-only lines
+ if not line.strip():
+ continue
+ # skip over comments
+ if line.startswith('#'):
+ continue
+ try:
+ type_, remainder = line.split(';', 1)
+ except ValueError:
+ raise RuntimeError(
+ "Line %d in '%s' doesn't contain a semicolon separating the "
+ 'type from the arguments' % (i + 1, dsv_path))
+ if type_ != DSV_TYPE_SOURCE:
+ # handle non-source lines
+ try:
+ commands += handle_dsv_types_except_source(
+ type_, remainder, prefix)
+ except RuntimeError as e:
+ raise RuntimeError(
+ "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e
+ else:
+ # group remaining source lines by basename
+ path_without_ext, ext = os.path.splitext(remainder)
+ if path_without_ext not in basenames:
+ basenames[path_without_ext] = set()
+ assert ext.startswith('.')
+ ext = ext[1:]
+ if ext in (primary_extension, additional_extension):
+ basenames[path_without_ext].add(ext)
+
+ # add the dsv extension to each basename if the file exists
+ for basename, extensions in basenames.items():
+ if not os.path.isabs(basename):
+ basename = os.path.join(prefix, basename)
+ if os.path.exists(basename + '.dsv'):
+ extensions.add('dsv')
+
+ for basename, extensions in basenames.items():
+ if not os.path.isabs(basename):
+ basename = os.path.join(prefix, basename)
+ if 'dsv' in extensions:
+ # process dsv files recursively
+ commands += process_dsv_file(
+ basename + '.dsv', prefix, primary_extension=primary_extension,
+ additional_extension=additional_extension)
+ elif primary_extension in extensions and len(extensions) == 1:
+ # source primary-only files
+ commands += [
+ FORMAT_STR_INVOKE_SCRIPT.format_map({
+ 'prefix': prefix,
+ 'script_path': basename + '.' + primary_extension})]
+ elif additional_extension in extensions:
+ # source non-primary files
+ commands += [
+ FORMAT_STR_INVOKE_SCRIPT.format_map({
+ 'prefix': prefix,
+ 'script_path': basename + '.' + additional_extension})]
+
+ return commands
+
+
+def handle_dsv_types_except_source(type_, remainder, prefix):
+ commands = []
+ if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET):
+ try:
+ env_name, value = remainder.split(';', 1)
+ except ValueError:
+ raise RuntimeError(
+ "doesn't contain a semicolon separating the environment name "
+ 'from the value')
+ try_prefixed_value = os.path.join(prefix, value) if value else prefix
+ if os.path.exists(try_prefixed_value):
+ value = try_prefixed_value
+ if type_ == DSV_TYPE_SET:
+ commands += _set(env_name, value)
+ elif type_ == DSV_TYPE_SET_IF_UNSET:
+ commands += _set_if_unset(env_name, value)
+ else:
+ assert False
+ elif type_ in (
+ DSV_TYPE_APPEND_NON_DUPLICATE,
+ DSV_TYPE_PREPEND_NON_DUPLICATE,
+ DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS
+ ):
+ try:
+ env_name_and_values = remainder.split(';')
+ except ValueError:
+ raise RuntimeError(
+ "doesn't contain a semicolon separating the environment name "
+ 'from the values')
+ env_name = env_name_and_values[0]
+ values = env_name_and_values[1:]
+ for value in values:
+ if not value:
+ value = prefix
+ elif not os.path.isabs(value):
+ value = os.path.join(prefix, value)
+ if (
+ type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and
+ not os.path.exists(value)
+ ):
+ comment = f'skip extending {env_name} with not existing ' \
+ f'path: {value}'
+ if _include_comments():
+ commands.append(
+ FORMAT_STR_COMMENT_LINE.format_map({'comment': comment}))
+ elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE:
+ commands += _append_unique_value(env_name, value)
+ else:
+ commands += _prepend_unique_value(env_name, value)
+ else:
+ raise RuntimeError(
+ 'contains an unknown environment hook type: ' + type_)
+ return commands
+
+
+env_state = {}
+
+
+def _append_unique_value(name, value):
+ global env_state
+ if name not in env_state:
+ if os.environ.get(name):
+ env_state[name] = set(os.environ[name].split(os.pathsep))
+ else:
+ env_state[name] = set()
+ # append even if the variable has not been set yet, in case a shell script sets the
+ # same variable without the knowledge of this Python script.
+ # later _remove_ending_separators() will cleanup any unintentional leading separator
+ extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': extend + value})
+ if value not in env_state[name]:
+ env_state[name].add(value)
+ else:
+ if not _include_comments():
+ return []
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+def _prepend_unique_value(name, value):
+ global env_state
+ if name not in env_state:
+ if os.environ.get(name):
+ env_state[name] = set(os.environ[name].split(os.pathsep))
+ else:
+ env_state[name] = set()
+ # prepend even if the variable has not been set yet, in case a shell script sets the
+ # same variable without the knowledge of this Python script.
+ # later _remove_ending_separators() will cleanup any unintentional trailing separator
+ extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name})
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value + extend})
+ if value not in env_state[name]:
+ env_state[name].add(value)
+ else:
+ if not _include_comments():
+ return []
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+# generate commands for removing prepended underscores
+def _remove_ending_separators():
+ # do nothing if the shell extension does not implement the logic
+ if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None:
+ return []
+
+ global env_state
+ commands = []
+ for name in env_state:
+ # skip variables that already had values before this script started prepending
+ if name in os.environ:
+ continue
+ commands += [
+ FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}),
+ FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})]
+ return commands
+
+
+def _set(name, value):
+ global env_state
+ env_state[name] = value
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value})
+ return [line]
+
+
+def _set_if_unset(name, value):
+ global env_state
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value})
+ if env_state.get(name, os.environ.get(name)):
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+if __name__ == '__main__': # pragma: no cover
+ try:
+ rc = main()
+ except RuntimeError as e:
+ print(str(e), file=sys.stderr)
+ rc = 1
+ sys.exit(rc)
diff --git a/install/_local_setup_util_sh.py b/install/_local_setup_util_sh.py
new file mode 100644
index 00000000..f67eaa98
--- /dev/null
+++ b/install/_local_setup_util_sh.py
@@ -0,0 +1,407 @@
+# Copyright 2016-2019 Dirk Thomas
+# Licensed under the Apache License, Version 2.0
+
+import argparse
+from collections import OrderedDict
+import os
+from pathlib import Path
+import sys
+
+
+FORMAT_STR_COMMENT_LINE = '# {comment}'
+FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"'
+FORMAT_STR_USE_ENV_VAR = '${name}'
+FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' # noqa: E501
+FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501
+FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501
+
+DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate'
+DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate'
+DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists'
+DSV_TYPE_SET = 'set'
+DSV_TYPE_SET_IF_UNSET = 'set-if-unset'
+DSV_TYPE_SOURCE = 'source'
+
+
+def main(argv=sys.argv[1:]): # noqa: D103
+ parser = argparse.ArgumentParser(
+ description='Output shell commands for the packages in topological '
+ 'order')
+ parser.add_argument(
+ 'primary_extension',
+ help='The file extension of the primary shell')
+ parser.add_argument(
+ 'additional_extension', nargs='?',
+ help='The additional file extension to be considered')
+ parser.add_argument(
+ '--merged-install', action='store_true',
+ help='All install prefixes are merged into a single location')
+ args = parser.parse_args(argv)
+
+ packages = get_packages(Path(__file__).parent, args.merged_install)
+
+ ordered_packages = order_packages(packages)
+ for pkg_name in ordered_packages:
+ if _include_comments():
+ print(
+ FORMAT_STR_COMMENT_LINE.format_map(
+ {'comment': 'Package: ' + pkg_name}))
+ prefix = os.path.abspath(os.path.dirname(__file__))
+ if not args.merged_install:
+ prefix = os.path.join(prefix, pkg_name)
+ for line in get_commands(
+ pkg_name, prefix, args.primary_extension,
+ args.additional_extension
+ ):
+ print(line)
+
+ for line in _remove_ending_separators():
+ print(line)
+
+
+def get_packages(prefix_path, merged_install):
+ """
+ Find packages based on colcon-specific files created during installation.
+
+ :param Path prefix_path: The install prefix path of all packages
+ :param bool merged_install: The flag if the packages are all installed
+ directly in the prefix or if each package is installed in a subdirectory
+ named after the package
+ :returns: A mapping from the package name to the set of runtime
+ dependencies
+ :rtype: dict
+ """
+ packages = {}
+ # since importing colcon_core isn't feasible here the following constant
+ # must match colcon_core.location.get_relative_package_index_path()
+ subdirectory = 'share/colcon-core/packages'
+ if merged_install:
+ # return if workspace is empty
+ if not (prefix_path / subdirectory).is_dir():
+ return packages
+ # find all files in the subdirectory
+ for p in (prefix_path / subdirectory).iterdir():
+ if not p.is_file():
+ continue
+ if p.name.startswith('.'):
+ continue
+ add_package_runtime_dependencies(p, packages)
+ else:
+ # for each subdirectory look for the package specific file
+ for p in prefix_path.iterdir():
+ if not p.is_dir():
+ continue
+ if p.name.startswith('.'):
+ continue
+ p = p / subdirectory / p.name
+ if p.is_file():
+ add_package_runtime_dependencies(p, packages)
+
+ # remove unknown dependencies
+ pkg_names = set(packages.keys())
+ for k in packages.keys():
+ packages[k] = {d for d in packages[k] if d in pkg_names}
+
+ return packages
+
+
+def add_package_runtime_dependencies(path, packages):
+ """
+ Check the path and if it exists extract the packages runtime dependencies.
+
+ :param Path path: The resource file containing the runtime dependencies
+ :param dict packages: A mapping from package names to the sets of runtime
+ dependencies to add to
+ """
+ content = path.read_text()
+ dependencies = set(content.split(os.pathsep) if content else [])
+ packages[path.name] = dependencies
+
+
+def order_packages(packages):
+ """
+ Order packages topologically.
+
+ :param dict packages: A mapping from package name to the set of runtime
+ dependencies
+ :returns: The package names
+ :rtype: list
+ """
+ # select packages with no dependencies in alphabetical order
+ to_be_ordered = list(packages.keys())
+ ordered = []
+ while to_be_ordered:
+ pkg_names_without_deps = [
+ name for name in to_be_ordered if not packages[name]]
+ if not pkg_names_without_deps:
+ reduce_cycle_set(packages)
+ raise RuntimeError(
+ 'Circular dependency between: ' + ', '.join(sorted(packages)))
+ pkg_names_without_deps.sort()
+ pkg_name = pkg_names_without_deps[0]
+ to_be_ordered.remove(pkg_name)
+ ordered.append(pkg_name)
+ # remove item from dependency lists
+ for k in list(packages.keys()):
+ if pkg_name in packages[k]:
+ packages[k].remove(pkg_name)
+ return ordered
+
+
+def reduce_cycle_set(packages):
+ """
+ Reduce the set of packages to the ones part of the circular dependency.
+
+ :param dict packages: A mapping from package name to the set of runtime
+ dependencies which is modified in place
+ """
+ last_depended = None
+ while len(packages) > 0:
+ # get all remaining dependencies
+ depended = set()
+ for pkg_name, dependencies in packages.items():
+ depended = depended.union(dependencies)
+ # remove all packages which are not dependent on
+ for name in list(packages.keys()):
+ if name not in depended:
+ del packages[name]
+ if last_depended:
+ # if remaining packages haven't changed return them
+ if last_depended == depended:
+ return packages.keys()
+ # otherwise reduce again
+ last_depended = depended
+
+
+def _include_comments():
+ # skipping comment lines when COLCON_TRACE is not set speeds up the
+ # processing especially on Windows
+ return bool(os.environ.get('COLCON_TRACE'))
+
+
+def get_commands(pkg_name, prefix, primary_extension, additional_extension):
+ commands = []
+ package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv')
+ if os.path.exists(package_dsv_path):
+ commands += process_dsv_file(
+ package_dsv_path, prefix, primary_extension, additional_extension)
+ return commands
+
+
+def process_dsv_file(
+ dsv_path, prefix, primary_extension=None, additional_extension=None
+):
+ commands = []
+ if _include_comments():
+ commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path}))
+ with open(dsv_path, 'r') as h:
+ content = h.read()
+ lines = content.splitlines()
+
+ basenames = OrderedDict()
+ for i, line in enumerate(lines):
+ # skip over empty or whitespace-only lines
+ if not line.strip():
+ continue
+ # skip over comments
+ if line.startswith('#'):
+ continue
+ try:
+ type_, remainder = line.split(';', 1)
+ except ValueError:
+ raise RuntimeError(
+ "Line %d in '%s' doesn't contain a semicolon separating the "
+ 'type from the arguments' % (i + 1, dsv_path))
+ if type_ != DSV_TYPE_SOURCE:
+ # handle non-source lines
+ try:
+ commands += handle_dsv_types_except_source(
+ type_, remainder, prefix)
+ except RuntimeError as e:
+ raise RuntimeError(
+ "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e
+ else:
+ # group remaining source lines by basename
+ path_without_ext, ext = os.path.splitext(remainder)
+ if path_without_ext not in basenames:
+ basenames[path_without_ext] = set()
+ assert ext.startswith('.')
+ ext = ext[1:]
+ if ext in (primary_extension, additional_extension):
+ basenames[path_without_ext].add(ext)
+
+ # add the dsv extension to each basename if the file exists
+ for basename, extensions in basenames.items():
+ if not os.path.isabs(basename):
+ basename = os.path.join(prefix, basename)
+ if os.path.exists(basename + '.dsv'):
+ extensions.add('dsv')
+
+ for basename, extensions in basenames.items():
+ if not os.path.isabs(basename):
+ basename = os.path.join(prefix, basename)
+ if 'dsv' in extensions:
+ # process dsv files recursively
+ commands += process_dsv_file(
+ basename + '.dsv', prefix, primary_extension=primary_extension,
+ additional_extension=additional_extension)
+ elif primary_extension in extensions and len(extensions) == 1:
+ # source primary-only files
+ commands += [
+ FORMAT_STR_INVOKE_SCRIPT.format_map({
+ 'prefix': prefix,
+ 'script_path': basename + '.' + primary_extension})]
+ elif additional_extension in extensions:
+ # source non-primary files
+ commands += [
+ FORMAT_STR_INVOKE_SCRIPT.format_map({
+ 'prefix': prefix,
+ 'script_path': basename + '.' + additional_extension})]
+
+ return commands
+
+
+def handle_dsv_types_except_source(type_, remainder, prefix):
+ commands = []
+ if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET):
+ try:
+ env_name, value = remainder.split(';', 1)
+ except ValueError:
+ raise RuntimeError(
+ "doesn't contain a semicolon separating the environment name "
+ 'from the value')
+ try_prefixed_value = os.path.join(prefix, value) if value else prefix
+ if os.path.exists(try_prefixed_value):
+ value = try_prefixed_value
+ if type_ == DSV_TYPE_SET:
+ commands += _set(env_name, value)
+ elif type_ == DSV_TYPE_SET_IF_UNSET:
+ commands += _set_if_unset(env_name, value)
+ else:
+ assert False
+ elif type_ in (
+ DSV_TYPE_APPEND_NON_DUPLICATE,
+ DSV_TYPE_PREPEND_NON_DUPLICATE,
+ DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS
+ ):
+ try:
+ env_name_and_values = remainder.split(';')
+ except ValueError:
+ raise RuntimeError(
+ "doesn't contain a semicolon separating the environment name "
+ 'from the values')
+ env_name = env_name_and_values[0]
+ values = env_name_and_values[1:]
+ for value in values:
+ if not value:
+ value = prefix
+ elif not os.path.isabs(value):
+ value = os.path.join(prefix, value)
+ if (
+ type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and
+ not os.path.exists(value)
+ ):
+ comment = f'skip extending {env_name} with not existing ' \
+ f'path: {value}'
+ if _include_comments():
+ commands.append(
+ FORMAT_STR_COMMENT_LINE.format_map({'comment': comment}))
+ elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE:
+ commands += _append_unique_value(env_name, value)
+ else:
+ commands += _prepend_unique_value(env_name, value)
+ else:
+ raise RuntimeError(
+ 'contains an unknown environment hook type: ' + type_)
+ return commands
+
+
+env_state = {}
+
+
+def _append_unique_value(name, value):
+ global env_state
+ if name not in env_state:
+ if os.environ.get(name):
+ env_state[name] = set(os.environ[name].split(os.pathsep))
+ else:
+ env_state[name] = set()
+ # append even if the variable has not been set yet, in case a shell script sets the
+ # same variable without the knowledge of this Python script.
+ # later _remove_ending_separators() will cleanup any unintentional leading separator
+ extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': extend + value})
+ if value not in env_state[name]:
+ env_state[name].add(value)
+ else:
+ if not _include_comments():
+ return []
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+def _prepend_unique_value(name, value):
+ global env_state
+ if name not in env_state:
+ if os.environ.get(name):
+ env_state[name] = set(os.environ[name].split(os.pathsep))
+ else:
+ env_state[name] = set()
+ # prepend even if the variable has not been set yet, in case a shell script sets the
+ # same variable without the knowledge of this Python script.
+ # later _remove_ending_separators() will cleanup any unintentional trailing separator
+ extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name})
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value + extend})
+ if value not in env_state[name]:
+ env_state[name].add(value)
+ else:
+ if not _include_comments():
+ return []
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+# generate commands for removing prepended underscores
+def _remove_ending_separators():
+ # do nothing if the shell extension does not implement the logic
+ if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None:
+ return []
+
+ global env_state
+ commands = []
+ for name in env_state:
+ # skip variables that already had values before this script started prepending
+ if name in os.environ:
+ continue
+ commands += [
+ FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}),
+ FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})]
+ return commands
+
+
+def _set(name, value):
+ global env_state
+ env_state[name] = value
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value})
+ return [line]
+
+
+def _set_if_unset(name, value):
+ global env_state
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value})
+ if env_state.get(name, os.environ.get(name)):
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+if __name__ == '__main__': # pragma: no cover
+ try:
+ rc = main()
+ except RuntimeError as e:
+ print(str(e), file=sys.stderr)
+ rc = 1
+ sys.exit(rc)
diff --git a/install/autonomous_kart/share/ament_index/resource_index/packages/autonomous_kart b/install/autonomous_kart/share/ament_index/resource_index/packages/autonomous_kart
new file mode 100644
index 00000000..e69de29b
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.dsv b/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.dsv
new file mode 100644
index 00000000..79d4c95b
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.dsv
@@ -0,0 +1 @@
+prepend-non-duplicate;AMENT_PREFIX_PATH;
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.ps1 b/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.ps1
new file mode 100644
index 00000000..26b99975
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.ps1
@@ -0,0 +1,3 @@
+# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
+
+colcon_prepend_unique_value AMENT_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX"
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.sh b/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.sh
new file mode 100644
index 00000000..f3041f68
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.sh
@@ -0,0 +1,3 @@
+# generated from colcon_core/shell/template/hook_prepend_value.sh.em
+
+_colcon_prepend_unique_value AMENT_PREFIX_PATH "$COLCON_CURRENT_PREFIX"
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.dsv b/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.dsv
new file mode 100644
index 00000000..257067d4
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.dsv
@@ -0,0 +1 @@
+prepend-non-duplicate;PYTHONPATH;lib/python3.10/site-packages
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.ps1 b/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.ps1
new file mode 100644
index 00000000..caffe83f
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.ps1
@@ -0,0 +1,3 @@
+# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
+
+colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX\lib/python3.10/site-packages"
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.sh b/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.sh
new file mode 100644
index 00000000..660c3483
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.sh
@@ -0,0 +1,3 @@
+# generated from colcon_core/shell/template/hook_prepend_value.sh.em
+
+_colcon_prepend_unique_value PYTHONPATH "$COLCON_CURRENT_PREFIX/lib/python3.10/site-packages"
diff --git a/install/autonomous_kart/share/autonomous_kart/launch/bringup_pi.launch.py b/install/autonomous_kart/share/autonomous_kart/launch/bringup_pi.launch.py
new file mode 100644
index 00000000..88a2aff8
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/launch/bringup_pi.launch.py
@@ -0,0 +1,49 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import os
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+ pkg_share = get_package_share_directory('autonomous_kart')
+
+ return LaunchDescription([
+ Node(
+ package='autonomous_kart',
+ executable='motor_node',
+ name='motor_node',
+ parameters=[os.path.join(pkg_share, 'params', 'controller.yaml'), {'simulation_mode': False}]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='steering_node',
+ name='steering_node',
+ parameters=[os.path.join(pkg_share, 'params', 'controller.yaml')]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='camera_node',
+ name='camera_node',
+ parameters=[os.path.join(pkg_share, 'params', 'camera.yaml')]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='gps_node',
+ name='gps_node',
+ parameters=[os.path.join(pkg_share, 'params', 'gps.yaml')]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='pathfinder_node',
+ name='pathfinder_node',
+ parameters=[os.path.join(pkg_share, 'params', 'planner.yaml'),
+ os.path.join(pkg_share, 'params', 'safety.yaml'), os.path.join(pkg_share, 'params', 'gps.yaml')]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='opencv_pathfinder_node',
+ name='opencv_pathfinder_node',
+ # parameters=[os.path.join(pkg_share, 'params', 'planner.yaml'),
+ # os.path.join(pkg_share, 'params', 'safety.yaml'), os.path.join(pkg_share, 'params', 'gps.yaml')]
+ ),
+ ])
diff --git a/install/autonomous_kart/share/autonomous_kart/launch/bringup_sim.launch.py b/install/autonomous_kart/share/autonomous_kart/launch/bringup_sim.launch.py
new file mode 100644
index 00000000..3e8f0212
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/launch/bringup_sim.launch.py
@@ -0,0 +1,45 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import os
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+ pkg_share = get_package_share_directory('autonomous_kart')
+
+ return LaunchDescription([
+ Node(
+ package='autonomous_kart',
+ executable='motor_node',
+ name='motor_node',
+ parameters=[os.path.join(pkg_share, 'params', 'controller.yaml'), {'simulation_mode': True}]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='steering_node',
+ name='steering_node',
+ parameters=[os.path.join(pkg_share, 'params', 'controller.yaml'), {'simulation_mode': True}]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='camera_node',
+ name='camera_node',
+ parameters=[os.path.join(pkg_share, 'params', 'camera.yaml'), {'simulation_mode': True}]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='pathfinder_node',
+ name='pathfinder_node',
+ parameters=[os.path.join(pkg_share, 'params', 'planner.yaml'),
+ os.path.join(pkg_share, 'params', 'safety.yaml'), os.path.join(pkg_share, 'params', 'gps.yaml'),
+ {'simulation_mode': True}]
+ ),
+ Node(
+ package='autonomous_kart',
+ executable='opencv_pathfinder_node',
+ name='opencv_pathfinder_node',
+ parameters=[os.path.join(pkg_share, 'params', 'planner.yaml'),
+ os.path.join(pkg_share, 'params', 'safety.yaml'), os.path.join(pkg_share, 'params', 'gps.yaml'),
+ {'simulation_mode': True}]
+ ),
+ ])
diff --git a/install/autonomous_kart/share/autonomous_kart/package.bash b/install/autonomous_kart/share/autonomous_kart/package.bash
new file mode 100644
index 00000000..48271952
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/package.bash
@@ -0,0 +1,31 @@
+# generated from colcon_bash/shell/template/package.bash.em
+
+# This script extends the environment for this package.
+
+# a bash script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ # the prefix is two levels up from the package specific share directory
+ _colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)"
+else
+ _colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_bash_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$@"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# source sh script of this package
+_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/autonomous_kart/package.sh"
+
+unset _colcon_package_bash_source_script
+unset _colcon_package_bash_COLCON_CURRENT_PREFIX
diff --git a/install/autonomous_kart/share/autonomous_kart/package.dsv b/install/autonomous_kart/share/autonomous_kart/package.dsv
new file mode 100644
index 00000000..b9a0734d
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/package.dsv
@@ -0,0 +1,6 @@
+source;share/autonomous_kart/hook/pythonpath.ps1
+source;share/autonomous_kart/hook/pythonpath.dsv
+source;share/autonomous_kart/hook/pythonpath.sh
+source;share/autonomous_kart/hook/ament_prefix_path.ps1
+source;share/autonomous_kart/hook/ament_prefix_path.dsv
+source;share/autonomous_kart/hook/ament_prefix_path.sh
diff --git a/install/autonomous_kart/share/autonomous_kart/package.ps1 b/install/autonomous_kart/share/autonomous_kart/package.ps1
new file mode 100644
index 00000000..3871215d
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/package.ps1
@@ -0,0 +1,116 @@
+# generated from colcon_powershell/shell/template/package.ps1.em
+
+# function to append a value to a variable
+# which uses colons as separators
+# duplicates as well as leading separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+function colcon_append_unique_value {
+ param (
+ $_listname,
+ $_value
+ )
+
+ # get values from variable
+ if (Test-Path Env:$_listname) {
+ $_values=(Get-Item env:$_listname).Value
+ } else {
+ $_values=""
+ }
+ $_duplicate=""
+ # start with no values
+ $_all_values=""
+ # iterate over existing values in the variable
+ if ($_values) {
+ $_values.Split(";") | ForEach {
+ # not an empty string
+ if ($_) {
+ # not a duplicate of _value
+ if ($_ -eq $_value) {
+ $_duplicate="1"
+ }
+ if ($_all_values) {
+ $_all_values="${_all_values};$_"
+ } else {
+ $_all_values="$_"
+ }
+ }
+ }
+ }
+ # append only non-duplicates
+ if (!$_duplicate) {
+ # avoid leading separator
+ if ($_all_values) {
+ $_all_values="${_all_values};${_value}"
+ } else {
+ $_all_values="${_value}"
+ }
+ }
+
+ # export the updated variable
+ Set-Item env:\$_listname -Value "$_all_values"
+}
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+function colcon_prepend_unique_value {
+ param (
+ $_listname,
+ $_value
+ )
+
+ # get values from variable
+ if (Test-Path Env:$_listname) {
+ $_values=(Get-Item env:$_listname).Value
+ } else {
+ $_values=""
+ }
+ # start with the new value
+ $_all_values="$_value"
+ # iterate over existing values in the variable
+ if ($_values) {
+ $_values.Split(";") | ForEach {
+ # not an empty string
+ if ($_) {
+ # not a duplicate of _value
+ if ($_ -ne $_value) {
+ # keep non-duplicate values
+ $_all_values="${_all_values};$_"
+ }
+ }
+ }
+ }
+ # export the updated variable
+ Set-Item env:\$_listname -Value "$_all_values"
+}
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+function colcon_package_source_powershell_script {
+ param (
+ $_colcon_package_source_powershell_script
+ )
+ # source script with conditional trace output
+ if (Test-Path $_colcon_package_source_powershell_script) {
+ if ($env:COLCON_TRACE) {
+ echo ". '$_colcon_package_source_powershell_script'"
+ }
+ . "$_colcon_package_source_powershell_script"
+ } else {
+ Write-Error "not found: '$_colcon_package_source_powershell_script'"
+ }
+}
+
+
+# a powershell script is able to determine its own path
+# the prefix is two levels up from the package specific share directory
+$env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.FullName
+
+colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/autonomous_kart/hook/pythonpath.ps1"
+colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/autonomous_kart/hook/ament_prefix_path.ps1"
+
+Remove-Item Env:\COLCON_CURRENT_PREFIX
diff --git a/install/autonomous_kart/share/autonomous_kart/package.sh b/install/autonomous_kart/share/autonomous_kart/package.sh
new file mode 100644
index 00000000..d53dab10
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/package.sh
@@ -0,0 +1,87 @@
+# generated from colcon_core/shell/template/package.sh.em
+
+# This script extends the environment for this package.
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prepend_unique_value() {
+ # arguments
+ _listname="$1"
+ _value="$2"
+
+ # get values from variable
+ eval _values=\"\$$_listname\"
+ # backup the field separator
+ _colcon_prepend_unique_value_IFS=$IFS
+ IFS=":"
+ # start with the new value
+ _all_values="$_value"
+ # workaround SH_WORD_SPLIT not being set in zsh
+ if [ "$(command -v colcon_zsh_convert_to_array)" ]; then
+ colcon_zsh_convert_to_array _values
+ fi
+ # iterate over existing values in the variable
+ for _item in $_values; do
+ # ignore empty strings
+ if [ -z "$_item" ]; then
+ continue
+ fi
+ # ignore duplicates of _value
+ if [ "$_item" = "$_value" ]; then
+ continue
+ fi
+ # keep non-duplicate values
+ _all_values="$_all_values:$_item"
+ done
+ unset _item
+ # restore the field separator
+ IFS=$_colcon_prepend_unique_value_IFS
+ unset _colcon_prepend_unique_value_IFS
+ # export the updated variable
+ eval export $_listname=\"$_all_values\"
+ unset _all_values
+ unset _values
+
+ unset _value
+ unset _listname
+}
+
+# since a plain shell script can't determine its own path when being sourced
+# either use the provided COLCON_CURRENT_PREFIX
+# or fall back to the build time prefix (if it exists)
+_colcon_package_sh_COLCON_CURRENT_PREFIX="/ws/install/autonomous_kart"
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then
+ echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
+ unset _colcon_package_sh_COLCON_CURRENT_PREFIX
+ return 1
+ fi
+ COLCON_CURRENT_PREFIX="$_colcon_package_sh_COLCON_CURRENT_PREFIX"
+fi
+unset _colcon_package_sh_COLCON_CURRENT_PREFIX
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_sh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$@"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# source sh hooks
+_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/autonomous_kart/hook/pythonpath.sh"
+_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/autonomous_kart/hook/ament_prefix_path.sh"
+
+unset _colcon_package_sh_source_script
+unset COLCON_CURRENT_PREFIX
+
+# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks
diff --git a/install/autonomous_kart/share/autonomous_kart/package.xml b/install/autonomous_kart/share/autonomous_kart/package.xml
new file mode 100644
index 00000000..0005e101
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/package.xml
@@ -0,0 +1,23 @@
+
+
+
+ autonomous_kart
+ 0.0.1
+ Package containing all nodes for driving in different states.
+ root
+ Apache-2.0
+
+ ament_python
+
+ rclpy
+ geometry_msgs
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
\ No newline at end of file
diff --git a/install/autonomous_kart/share/autonomous_kart/package.zsh b/install/autonomous_kart/share/autonomous_kart/package.zsh
new file mode 100644
index 00000000..179a08f0
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/package.zsh
@@ -0,0 +1,42 @@
+# generated from colcon_zsh/shell/template/package.zsh.em
+
+# This script extends the environment for this package.
+
+# a zsh script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ # the prefix is two levels up from the package specific share directory
+ _colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)"
+else
+ _colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_zsh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$@"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# function to convert array-like strings into arrays
+# to workaround SH_WORD_SPLIT not being set
+colcon_zsh_convert_to_array() {
+ local _listname=$1
+ local _dollar="$"
+ local _split="{="
+ local _to_array="(\"$_dollar$_split$_listname}\")"
+ eval $_listname=$_to_array
+}
+
+# source sh script of this package
+_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/autonomous_kart/package.sh"
+unset convert_zsh_to_array
+
+unset _colcon_package_zsh_source_script
+unset _colcon_package_zsh_COLCON_CURRENT_PREFIX
diff --git a/install/autonomous_kart/share/autonomous_kart/params/camera.yaml b/install/autonomous_kart/share/autonomous_kart/params/camera.yaml
new file mode 100644
index 00000000..346227a4
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/params/camera.yaml
@@ -0,0 +1,5 @@
+# Camera config settings
+camera_node:
+ ros__parameters:
+ simulation_mode: false # set later
+ fps: 30.0 # float
\ No newline at end of file
diff --git a/install/autonomous_kart/share/autonomous_kart/params/controller.yaml b/install/autonomous_kart/share/autonomous_kart/params/controller.yaml
new file mode 100644
index 00000000..3bd8bf1a
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/params/controller.yaml
@@ -0,0 +1,8 @@
+# Params and toggles for RC controller drive including gain, rates, etc.
+# Dummy values for now
+motor_node:
+ ros__parameters:
+ max_linear_speed: 5.0 # m/s
+ max_angular_speed: 3.0 # rad/s
+ motor_timeout: 1.0 # seconds
+ simulation_mode: false # set in motor node
\ No newline at end of file
diff --git a/install/autonomous_kart/share/autonomous_kart/params/gps.yaml b/install/autonomous_kart/share/autonomous_kart/params/gps.yaml
new file mode 100644
index 00000000..e69de29b
diff --git a/install/autonomous_kart/share/autonomous_kart/params/planner.yaml b/install/autonomous_kart/share/autonomous_kart/params/planner.yaml
new file mode 100644
index 00000000..17ee3bcd
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/params/planner.yaml
@@ -0,0 +1 @@
+# Planner configs including perception thresholds, toggles, etc.
\ No newline at end of file
diff --git a/install/autonomous_kart/share/autonomous_kart/params/safety.yaml b/install/autonomous_kart/share/autonomous_kart/params/safety.yaml
new file mode 100644
index 00000000..39564898
--- /dev/null
+++ b/install/autonomous_kart/share/autonomous_kart/params/safety.yaml
@@ -0,0 +1 @@
+# Safety limits and estop config
\ No newline at end of file
diff --git a/install/autonomous_kart/share/colcon-core/packages/autonomous_kart b/install/autonomous_kart/share/colcon-core/packages/autonomous_kart
new file mode 100644
index 00000000..18e15c42
--- /dev/null
+++ b/install/autonomous_kart/share/colcon-core/packages/autonomous_kart
@@ -0,0 +1 @@
+geometry_msgs:rclpy
\ No newline at end of file
diff --git a/install/local_setup.bash b/install/local_setup.bash
new file mode 100644
index 00000000..03f00256
--- /dev/null
+++ b/install/local_setup.bash
@@ -0,0 +1,121 @@
+# generated from colcon_bash/shell/template/prefix.bash.em
+
+# This script extends the environment with all packages contained in this
+# prefix path.
+
+# a bash script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)"
+else
+ _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prefix_bash_prepend_unique_value() {
+ # arguments
+ _listname="$1"
+ _value="$2"
+
+ # get values from variable
+ eval _values=\"\$$_listname\"
+ # backup the field separator
+ _colcon_prefix_bash_prepend_unique_value_IFS="$IFS"
+ IFS=":"
+ # start with the new value
+ _all_values="$_value"
+ _contained_value=""
+ # iterate over existing values in the variable
+ for _item in $_values; do
+ # ignore empty strings
+ if [ -z "$_item" ]; then
+ continue
+ fi
+ # ignore duplicates of _value
+ if [ "$_item" = "$_value" ]; then
+ _contained_value=1
+ continue
+ fi
+ # keep non-duplicate values
+ _all_values="$_all_values:$_item"
+ done
+ unset _item
+ if [ -z "$_contained_value" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ if [ "$_all_values" = "$_value" ]; then
+ echo "export $_listname=$_value"
+ else
+ echo "export $_listname=$_value:\$$_listname"
+ fi
+ fi
+ fi
+ unset _contained_value
+ # restore the field separator
+ IFS="$_colcon_prefix_bash_prepend_unique_value_IFS"
+ unset _colcon_prefix_bash_prepend_unique_value_IFS
+ # export the updated variable
+ eval export $_listname=\"$_all_values\"
+ unset _all_values
+ unset _values
+
+ unset _value
+ unset _listname
+}
+
+# add this prefix to the COLCON_PREFIX_PATH
+_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX"
+unset _colcon_prefix_bash_prepend_unique_value
+
+# check environment variable for custom Python executable
+if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then
+ if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then
+ echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist"
+ return 1
+ fi
+ _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE"
+else
+ # try the Python executable known at configure time
+ _colcon_python_executable="/usr/bin/python3"
+ # if it doesn't exist try a fall back
+ if [ ! -f "$_colcon_python_executable" ]; then
+ if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then
+ echo "error: unable to find python3 executable"
+ return 1
+ fi
+ _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"`
+ fi
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_sh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# get all commands in topological order
+_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)"
+unset _colcon_python_executable
+if [ -n "$COLCON_TRACE" ]; then
+ echo "$(declare -f _colcon_prefix_sh_source_script)"
+ echo "# Execute generated script:"
+ echo "# <<<"
+ echo "${_colcon_ordered_commands}"
+ echo "# >>>"
+ echo "unset _colcon_prefix_sh_source_script"
+fi
+eval "${_colcon_ordered_commands}"
+unset _colcon_ordered_commands
+
+unset _colcon_prefix_sh_source_script
+
+unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX
diff --git a/install/local_setup.ps1 b/install/local_setup.ps1
new file mode 100644
index 00000000..6f68c8de
--- /dev/null
+++ b/install/local_setup.ps1
@@ -0,0 +1,55 @@
+# generated from colcon_powershell/shell/template/prefix.ps1.em
+
+# This script extends the environment with all packages contained in this
+# prefix path.
+
+# check environment variable for custom Python executable
+if ($env:COLCON_PYTHON_EXECUTABLE) {
+ if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) {
+ echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist"
+ exit 1
+ }
+ $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE"
+} else {
+ # use the Python executable known at configure time
+ $_colcon_python_executable="/usr/bin/python3"
+ # if it doesn't exist try a fall back
+ if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) {
+ if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) {
+ echo "error: unable to find python3 executable"
+ exit 1
+ }
+ $_colcon_python_executable="python3"
+ }
+}
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+function _colcon_prefix_powershell_source_script {
+ param (
+ $_colcon_prefix_powershell_source_script_param
+ )
+ # source script with conditional trace output
+ if (Test-Path $_colcon_prefix_powershell_source_script_param) {
+ if ($env:COLCON_TRACE) {
+ echo ". '$_colcon_prefix_powershell_source_script_param'"
+ }
+ . "$_colcon_prefix_powershell_source_script_param"
+ } else {
+ Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'"
+ }
+}
+
+# get all commands in topological order
+$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1
+
+# execute all commands in topological order
+if ($env:COLCON_TRACE) {
+ echo "Execute generated script:"
+ echo "<<<"
+ $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output
+ echo ">>>"
+}
+if ($_colcon_ordered_commands) {
+ $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression
+}
diff --git a/install/local_setup.sh b/install/local_setup.sh
new file mode 100644
index 00000000..6ddd1a7f
--- /dev/null
+++ b/install/local_setup.sh
@@ -0,0 +1,137 @@
+# generated from colcon_core/shell/template/prefix.sh.em
+
+# This script extends the environment with all packages contained in this
+# prefix path.
+
+# since a plain shell script can't determine its own path when being sourced
+# either use the provided COLCON_CURRENT_PREFIX
+# or fall back to the build time prefix (if it exists)
+_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/ws/install"
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then
+ echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
+ unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX
+ return 1
+ fi
+else
+ _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prefix_sh_prepend_unique_value() {
+ # arguments
+ _listname="$1"
+ _value="$2"
+
+ # get values from variable
+ eval _values=\"\$$_listname\"
+ # backup the field separator
+ _colcon_prefix_sh_prepend_unique_value_IFS="$IFS"
+ IFS=":"
+ # start with the new value
+ _all_values="$_value"
+ _contained_value=""
+ # iterate over existing values in the variable
+ for _item in $_values; do
+ # ignore empty strings
+ if [ -z "$_item" ]; then
+ continue
+ fi
+ # ignore duplicates of _value
+ if [ "$_item" = "$_value" ]; then
+ _contained_value=1
+ continue
+ fi
+ # keep non-duplicate values
+ _all_values="$_all_values:$_item"
+ done
+ unset _item
+ if [ -z "$_contained_value" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ if [ "$_all_values" = "$_value" ]; then
+ echo "export $_listname=$_value"
+ else
+ echo "export $_listname=$_value:\$$_listname"
+ fi
+ fi
+ fi
+ unset _contained_value
+ # restore the field separator
+ IFS="$_colcon_prefix_sh_prepend_unique_value_IFS"
+ unset _colcon_prefix_sh_prepend_unique_value_IFS
+ # export the updated variable
+ eval export $_listname=\"$_all_values\"
+ unset _all_values
+ unset _values
+
+ unset _value
+ unset _listname
+}
+
+# add this prefix to the COLCON_PREFIX_PATH
+_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX"
+unset _colcon_prefix_sh_prepend_unique_value
+
+# check environment variable for custom Python executable
+if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then
+ if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then
+ echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist"
+ return 1
+ fi
+ _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE"
+else
+ # try the Python executable known at configure time
+ _colcon_python_executable="/usr/bin/python3"
+ # if it doesn't exist try a fall back
+ if [ ! -f "$_colcon_python_executable" ]; then
+ if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then
+ echo "error: unable to find python3 executable"
+ return 1
+ fi
+ _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"`
+ fi
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_sh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# get all commands in topological order
+_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)"
+unset _colcon_python_executable
+if [ -n "$COLCON_TRACE" ]; then
+ echo "_colcon_prefix_sh_source_script() {
+ if [ -f \"\$1\" ]; then
+ if [ -n \"\$COLCON_TRACE\" ]; then
+ echo \"# . \\\"\$1\\\"\"
+ fi
+ . \"\$1\"
+ else
+ echo \"not found: \\\"\$1\\\"\" 1>&2
+ fi
+ }"
+ echo "# Execute generated script:"
+ echo "# <<<"
+ echo "${_colcon_ordered_commands}"
+ echo "# >>>"
+ echo "unset _colcon_prefix_sh_source_script"
+fi
+eval "${_colcon_ordered_commands}"
+unset _colcon_ordered_commands
+
+unset _colcon_prefix_sh_source_script
+
+unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX
diff --git a/install/local_setup.zsh b/install/local_setup.zsh
new file mode 100644
index 00000000..b6487102
--- /dev/null
+++ b/install/local_setup.zsh
@@ -0,0 +1,134 @@
+# generated from colcon_zsh/shell/template/prefix.zsh.em
+
+# This script extends the environment with all packages contained in this
+# prefix path.
+
+# a zsh script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)"
+else
+ _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to convert array-like strings into arrays
+# to workaround SH_WORD_SPLIT not being set
+_colcon_prefix_zsh_convert_to_array() {
+ local _listname=$1
+ local _dollar="$"
+ local _split="{="
+ local _to_array="(\"$_dollar$_split$_listname}\")"
+ eval $_listname=$_to_array
+}
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prefix_zsh_prepend_unique_value() {
+ # arguments
+ _listname="$1"
+ _value="$2"
+
+ # get values from variable
+ eval _values=\"\$$_listname\"
+ # backup the field separator
+ _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS"
+ IFS=":"
+ # start with the new value
+ _all_values="$_value"
+ _contained_value=""
+ # workaround SH_WORD_SPLIT not being set
+ _colcon_prefix_zsh_convert_to_array _values
+ # iterate over existing values in the variable
+ for _item in $_values; do
+ # ignore empty strings
+ if [ -z "$_item" ]; then
+ continue
+ fi
+ # ignore duplicates of _value
+ if [ "$_item" = "$_value" ]; then
+ _contained_value=1
+ continue
+ fi
+ # keep non-duplicate values
+ _all_values="$_all_values:$_item"
+ done
+ unset _item
+ if [ -z "$_contained_value" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ if [ "$_all_values" = "$_value" ]; then
+ echo "export $_listname=$_value"
+ else
+ echo "export $_listname=$_value:\$$_listname"
+ fi
+ fi
+ fi
+ unset _contained_value
+ # restore the field separator
+ IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS"
+ unset _colcon_prefix_zsh_prepend_unique_value_IFS
+ # export the updated variable
+ eval export $_listname=\"$_all_values\"
+ unset _all_values
+ unset _values
+
+ unset _value
+ unset _listname
+}
+
+# add this prefix to the COLCON_PREFIX_PATH
+_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX"
+unset _colcon_prefix_zsh_prepend_unique_value
+unset _colcon_prefix_zsh_convert_to_array
+
+# check environment variable for custom Python executable
+if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then
+ if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then
+ echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist"
+ return 1
+ fi
+ _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE"
+else
+ # try the Python executable known at configure time
+ _colcon_python_executable="/usr/bin/python3"
+ # if it doesn't exist try a fall back
+ if [ ! -f "$_colcon_python_executable" ]; then
+ if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then
+ echo "error: unable to find python3 executable"
+ return 1
+ fi
+ _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"`
+ fi
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_sh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# get all commands in topological order
+_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)"
+unset _colcon_python_executable
+if [ -n "$COLCON_TRACE" ]; then
+ echo "$(declare -f _colcon_prefix_sh_source_script)"
+ echo "# Execute generated script:"
+ echo "# <<<"
+ echo "${_colcon_ordered_commands}"
+ echo "# >>>"
+ echo "unset _colcon_prefix_sh_source_script"
+fi
+eval "${_colcon_ordered_commands}"
+unset _colcon_ordered_commands
+
+unset _colcon_prefix_sh_source_script
+
+unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX
diff --git a/install/setup.bash b/install/setup.bash
new file mode 100644
index 00000000..10ea0f7c
--- /dev/null
+++ b/install/setup.bash
@@ -0,0 +1,31 @@
+# generated from colcon_bash/shell/template/prefix_chain.bash.em
+
+# This script extends the environment with the environment of other prefix
+# paths which were sourced when this file was generated as well as all packages
+# contained in this prefix path.
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_chain_bash_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# source chained prefixes
+# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
+COLCON_CURRENT_PREFIX="/opt/ros/humble"
+_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
+
+# source this prefix
+# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
+COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)"
+_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
+
+unset COLCON_CURRENT_PREFIX
+unset _colcon_prefix_chain_bash_source_script
diff --git a/install/setup.ps1 b/install/setup.ps1
new file mode 100644
index 00000000..558e9b9e
--- /dev/null
+++ b/install/setup.ps1
@@ -0,0 +1,29 @@
+# generated from colcon_powershell/shell/template/prefix_chain.ps1.em
+
+# This script extends the environment with the environment of other prefix
+# paths which were sourced when this file was generated as well as all packages
+# contained in this prefix path.
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+function _colcon_prefix_chain_powershell_source_script {
+ param (
+ $_colcon_prefix_chain_powershell_source_script_param
+ )
+ # source script with conditional trace output
+ if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) {
+ if ($env:COLCON_TRACE) {
+ echo ". '$_colcon_prefix_chain_powershell_source_script_param'"
+ }
+ . "$_colcon_prefix_chain_powershell_source_script_param"
+ } else {
+ Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'"
+ }
+}
+
+# source chained prefixes
+_colcon_prefix_chain_powershell_source_script "/opt/ros/humble\local_setup.ps1"
+
+# source this prefix
+$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent)
+_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1"
diff --git a/install/setup.sh b/install/setup.sh
new file mode 100644
index 00000000..7a978ade
--- /dev/null
+++ b/install/setup.sh
@@ -0,0 +1,45 @@
+# generated from colcon_core/shell/template/prefix_chain.sh.em
+
+# This script extends the environment with the environment of other prefix
+# paths which were sourced when this file was generated as well as all packages
+# contained in this prefix path.
+
+# since a plain shell script can't determine its own path when being sourced
+# either use the provided COLCON_CURRENT_PREFIX
+# or fall back to the build time prefix (if it exists)
+_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/ws/install
+if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then
+ _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then
+ echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
+ unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX
+ return 1
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_chain_sh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# source chained prefixes
+# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script
+COLCON_CURRENT_PREFIX="/opt/ros/humble"
+_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh"
+
+
+# source this prefix
+# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script
+COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX"
+_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh"
+
+unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX
+unset _colcon_prefix_chain_sh_source_script
+unset COLCON_CURRENT_PREFIX
diff --git a/install/setup.zsh b/install/setup.zsh
new file mode 100644
index 00000000..54799fde
--- /dev/null
+++ b/install/setup.zsh
@@ -0,0 +1,31 @@
+# generated from colcon_zsh/shell/template/prefix_chain.zsh.em
+
+# This script extends the environment with the environment of other prefix
+# paths which were sourced when this file was generated as well as all packages
+# contained in this prefix path.
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_chain_zsh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# source chained prefixes
+# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
+COLCON_CURRENT_PREFIX="/opt/ros/humble"
+_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh"
+
+# source this prefix
+# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
+COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)"
+_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh"
+
+unset COLCON_CURRENT_PREFIX
+unset _colcon_prefix_chain_zsh_source_script
diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE
new file mode 100644
index 00000000..e69de29b
diff --git a/log/latest b/log/latest
new file mode 120000
index 00000000..b57d247c
--- /dev/null
+++ b/log/latest
@@ -0,0 +1 @@
+latest_build
\ No newline at end of file
diff --git a/log/latest_build b/log/latest_build
new file mode 120000
index 00000000..36f1f9ad
--- /dev/null
+++ b/log/latest_build
@@ -0,0 +1 @@
+build_2025-10-23_00-39-04
\ No newline at end of file
diff --git a/opencv_testing/pathfinder.py b/opencv_testing/pathfinder.py
index 22a52d5e..2e9157ea 100644
--- a/opencv_testing/pathfinder.py
+++ b/opencv_testing/pathfinder.py
@@ -3,7 +3,7 @@
speed = 0.0 # mph
max_accel = 3.0 # mph per second
-max_steering = 25 # degrees
+max_steering = 90 # degrees
time_step = 1 # seconds
theta1 = -5
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
index 5ee078f0..de813414 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
@@ -2,7 +2,39 @@
from typing import Tuple
-def pathfinder(opencv_output: Tuple):
+def pathfinder(opencv_output: Tuple, logger):
+
+ speed = 0.0 # mph
+ max_accel = 3.0 # mph per second
+ max_steering = 25 # degrees
+ time_step = 1 # seconds
+
+ theta1 = opencv_output[0]
+ theta2 = opencv_output[1]
+
+ desired_heading = (theta1 + theta2) / 2 #average of both angles
+
+ if abs(desired_heading) < 10:
+ target_speed = 30 #max speed on straights
+ else:
+ target_speed = 15 #target speed on turns
+
+ if speed < target_speed:
+ speed += max_accel * time_step #physics c: mechanics
+ if speed > target_speed:
+ speed = target_speed
+ else:
+ speed -= max_accel * time_step
+ if speed < target_speed:
+ speed = target_speed
+
+ steering_command = max(min(desired_heading, max_steering), -max_steering)
+
+ get_logger.info(f"Theta1: {theta1:.1f}°, Theta2: {theta2:.1f}°")
+ get_logger.info(f"Desired Heading: {desired_heading:.1f}°")
+ get_logger.info(f"Target Speed: {target_speed} mph | Current Speed: {speed:.1f} mph")
+ get_logger.info(f"Steering Command: {steering_command:.1f}° {'Left' if steering_command < 0 else 'Right' if steering_command > 0 else 'Straight'}")
+
"""
Calculate commands for steering and motor from opencv_pathfinder efficiently
Part of hot loop so must be efficient.
@@ -11,8 +43,8 @@ def pathfinder(opencv_output: Tuple):
:return: Returns commands to motor & steering in (speed % of total, steering angle (degrees from -90 to 90 with 0 as straight)
"""
- # Dummy
- motor_speed = 20 * random.random()
- steering_angle = 180 * random.random() - 90
+ Dummy
+ motor_speed = 7 #20 * random.random()
+ steering_angle = 7 #180 * random.random() - 90
- return motor_speed, steering_angle
\ No newline at end of file
+ return motor_speed, steering_angle
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
index 7415b082..e76a6623 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
@@ -1,6 +1,7 @@
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32MultiArray, Float32
+from std_msgs.msg import Float32MultiArray, Float32
from autonomous_kart.nodes.pathfinder.pathfinder import pathfinder
@@ -17,15 +18,20 @@ def __init__(self):
self.declare_parameter('system_frequency', 60)
self.system_frequency = self.get_parameter('system_frequency').value
+ self.declare_parameter('system_frequency', 60)
+ self.system_frequency = self.get_parameter('system_frequency').value
+
# Timer to log average every 5 seconds
self.create_timer(5.0, self.log_command_rate)
# Subscriber to opencv pathfinder for angles
self.opencv_pathfinder_subscriber = self.create_subscription(
+ Float32MultiArray,
Float32MultiArray,
'track_angles',
self.calculate_path_callback,
5
+ 5
)
# Publisher to motor
@@ -33,6 +39,7 @@ def __init__(self):
Float32,
'cmd_vel',
5
+ 5
)
# # Publisher to steering
@@ -40,21 +47,24 @@ def __init__(self):
Float32,
'cmd_turn',
5
+ 5
)
self.logger.info("Initialize Pathfinder Node")
+ def calculate_path_callback(self, msg: Float32MultiArray):
def calculate_path_callback(self, msg: Float32MultiArray):
"""
Calculate commands for steering and motor from opencv_pathfinder efficiently
Part of hot loop so must be efficient
:param msg: Float32MultiArray of [left angle from center to base of track from image, right angle ...]
+ :param msg: Float32MultiArray of [left angle from center to base of track from image, right angle ...]
:return: Publishes commands to motor & steering
"""
self.cmd_count += 1
self.angles = (msg.data[0], msg.data[1])
- motor_speed, steering_angle = pathfinder(msg.data)
+ motor_speed, steering_angle = pathfinder(msg.data, self.logger)
self.steering_publisher.publish(Float32(data=steering_angle))
self.motor_publisher.publish(Float32(data=motor_speed))
@@ -80,18 +90,17 @@ def main(args=None):
rclpy.init(args=args)
node = PathfinderNode()
-
try:
rclpy.spin(node)
+ rclpy.spin(node)
except KeyboardInterrupt:
pass
except Exception:
node.get_logger().error('Unhandled exception', exc_info=True)
finally:
node.destroy_node()
- if rclpy.ok():
- rclpy.shutdown()
+ rclpy.shutdown()
if __name__ == '__main__':
- main()
+ main()
\ No newline at end of file
From 7beae0be5fe41155461eb52ae99b1e38e4541b93 Mon Sep 17 00:00:00 2001
From: Hirthik Gopal Shanmugam <141051449+hgs2007@users.noreply.github.com>
Date: Tue, 4 Nov 2025 18:31:27 -0500
Subject: [PATCH 09/21] made sure angles were correct
---
.../autonomous_kart/nodes/pathfinder/pathfinder.py | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
index de813414..a6321648 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
@@ -9,7 +9,7 @@ def pathfinder(opencv_output: Tuple, logger):
max_steering = 25 # degrees
time_step = 1 # seconds
- theta1 = opencv_output[0]
+ theta1 = -1 * opencv_output[0]
theta2 = opencv_output[1]
desired_heading = (theta1 + theta2) / 2 #average of both angles
From 40444a24c1303eee4ae6ea017fd36374632b48d6 Mon Sep 17 00:00:00 2001
From: Hirthik Gopal Shanmugam <141051449+hgs2007@users.noreply.github.com>
Date: Wed, 5 Nov 2025 00:12:44 +0000
Subject: [PATCH 10/21] refactor pathfinder function to use logger parameter
and clean up code
---
.../opencv_pathfinder/angle_calculator.py | 2 +-
.../nodes/pathfinder/pathfinder.py | 49 +++----------------
2 files changed, 8 insertions(+), 43 deletions(-)
diff --git a/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py b/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py
index 90984b83..9d42c74c 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py
@@ -10,4 +10,4 @@ def calculate_track_angles(frame: cv2.Mat) -> Tuple[float, float]:
:param frame: Most recent image from camera
:return: Tuple of [left angle (degrees), right angle (degrees)]. For example, return 75.0, 75.0
"""
- return 75.0, 75.0 # dummy
\ No newline at end of file
+ return 67.0, 41.0 # dummy
\ No newline at end of file
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
index 99918d80..15f14125 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
@@ -2,39 +2,7 @@
from typing import Tuple
-def pathfinder(opencv_output: Tuple, logger, logger):
-
- speed = 0.0 # mph
- max_accel = 3.0 # mph per second
- max_steering = 25 # degrees
- time_step = 1 # seconds
-
- theta1 = opencv_output[0]
- theta2 = opencv_output[1]
-
- desired_heading = (theta1 + theta2) / 2 #average of both angles
-
- if abs(desired_heading) < 10:
- target_speed = 30 #max speed on straights
- else:
- target_speed = 15 #target speed on turns
-
- if speed < target_speed:
- speed += max_accel * time_step #physics c: mechanics
- if speed > target_speed:
- speed = target_speed
- else:
- speed -= max_accel * time_step
- if speed < target_speed:
- speed = target_speed
-
- steering_command = max(min(desired_heading, max_steering), -max_steering)
-
- get_logger.info(f"Theta1: {theta1:.1f}°, Theta2: {theta2:.1f}°")
- get_logger.info(f"Desired Heading: {desired_heading:.1f}°")
- get_logger.info(f"Target Speed: {target_speed} mph | Current Speed: {speed:.1f} mph")
- get_logger.info(f"Steering Command: {steering_command:.1f}° {'Left' if steering_command < 0 else 'Right' if steering_command > 0 else 'Straight'}")
-
+def pathfinder(opencv_output: Tuple, logger):
speed = 0.0 # mph
max_accel = 3.0 # mph per second
@@ -62,10 +30,11 @@ def pathfinder(opencv_output: Tuple, logger, logger):
steering_command = max(min(desired_heading, max_steering), -max_steering)
- get_logger.info(f"Theta1: {theta1:.1f}°, Theta2: {theta2:.1f}°")
- get_logger.info(f"Desired Heading: {desired_heading:.1f}°")
- get_logger.info(f"Target Speed: {target_speed} mph | Current Speed: {speed:.1f} mph")
- get_logger.info(f"Steering Command: {steering_command:.1f}° {'Left' if steering_command < 0 else 'Right' if steering_command > 0 else 'Straight'}")
+ logger.info(f"Theta1: {theta1:.1f}°, Theta2: {theta2:.1f}°")
+ logger.info(f"Desired Heading: {desired_heading:.1f}°")
+ logger.info(f"Target Speed: {target_speed} mph | Current Speed: {speed:.1f} mph")
+ logger.info(f"Steering Command: {steering_command:.1f}° {'Left' if steering_command < 0 else 'Right' if steering_command > 0 else 'Straight'}")
+
"""
Calculate commands for steering and motor from opencv_pathfinder efficiently
@@ -75,8 +44,4 @@ def pathfinder(opencv_output: Tuple, logger, logger):
:return: Returns commands to motor & steering in (speed % of total, steering angle (degrees from -90 to 90 with 0 as straight)
"""
- Dummy
- motor_speed = 7 #20 * random.random()
- steering_angle = 7 #180 * random.random() - 90
-
- return motor_speed, steering_angle
+ return float(speed), float(steering_command)
From 619d226168c81aaa6afa83f0c534e51ffba53be2 Mon Sep 17 00:00:00 2001
From: shb-png
Date: Wed, 5 Nov 2025 06:15:38 -0500
Subject: [PATCH 11/21] remove ignored files
---
install/.colcon_install_layout | 1 -
install/COLCON_IGNORE | 0
install/_local_setup_util_ps1.py | 407 ------------------
install/_local_setup_util_sh.py | 407 ------------------
.../resource_index/packages/autonomous_kart | 0
.../hook/ament_prefix_path.dsv | 1 -
.../hook/ament_prefix_path.ps1 | 3 -
.../autonomous_kart/hook/ament_prefix_path.sh | 3 -
.../share/autonomous_kart/hook/pythonpath.dsv | 1 -
.../share/autonomous_kart/hook/pythonpath.ps1 | 3 -
.../share/autonomous_kart/hook/pythonpath.sh | 3 -
.../launch/bringup_pi.launch.py | 49 ---
.../launch/bringup_sim.launch.py | 45 --
.../share/autonomous_kart/package.bash | 31 --
.../share/autonomous_kart/package.dsv | 6 -
.../share/autonomous_kart/package.ps1 | 116 -----
.../share/autonomous_kart/package.sh | 87 ----
.../share/autonomous_kart/package.xml | 23 -
.../share/autonomous_kart/package.zsh | 42 --
.../share/autonomous_kart/params/camera.yaml | 5 -
.../autonomous_kart/params/controller.yaml | 8 -
.../share/autonomous_kart/params/gps.yaml | 0
.../share/autonomous_kart/params/planner.yaml | 1 -
.../share/autonomous_kart/params/safety.yaml | 1 -
.../colcon-core/packages/autonomous_kart | 1 -
install/local_setup.bash | 121 ------
install/local_setup.ps1 | 55 ---
install/local_setup.sh | 137 ------
install/local_setup.zsh | 134 ------
install/setup.bash | 31 --
install/setup.ps1 | 29 --
install/setup.sh | 45 --
install/setup.zsh | 31 --
log/COLCON_IGNORE | 0
log/latest | 1 -
log/latest_build | 1 -
36 files changed, 1829 deletions(-)
delete mode 100644 install/.colcon_install_layout
delete mode 100644 install/COLCON_IGNORE
delete mode 100644 install/_local_setup_util_ps1.py
delete mode 100644 install/_local_setup_util_sh.py
delete mode 100644 install/autonomous_kart/share/ament_index/resource_index/packages/autonomous_kart
delete mode 100644 install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.dsv
delete mode 100644 install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.ps1
delete mode 100644 install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.sh
delete mode 100644 install/autonomous_kart/share/autonomous_kart/hook/pythonpath.dsv
delete mode 100644 install/autonomous_kart/share/autonomous_kart/hook/pythonpath.ps1
delete mode 100644 install/autonomous_kart/share/autonomous_kart/hook/pythonpath.sh
delete mode 100644 install/autonomous_kart/share/autonomous_kart/launch/bringup_pi.launch.py
delete mode 100644 install/autonomous_kart/share/autonomous_kart/launch/bringup_sim.launch.py
delete mode 100644 install/autonomous_kart/share/autonomous_kart/package.bash
delete mode 100644 install/autonomous_kart/share/autonomous_kart/package.dsv
delete mode 100644 install/autonomous_kart/share/autonomous_kart/package.ps1
delete mode 100644 install/autonomous_kart/share/autonomous_kart/package.sh
delete mode 100644 install/autonomous_kart/share/autonomous_kart/package.xml
delete mode 100644 install/autonomous_kart/share/autonomous_kart/package.zsh
delete mode 100644 install/autonomous_kart/share/autonomous_kart/params/camera.yaml
delete mode 100644 install/autonomous_kart/share/autonomous_kart/params/controller.yaml
delete mode 100644 install/autonomous_kart/share/autonomous_kart/params/gps.yaml
delete mode 100644 install/autonomous_kart/share/autonomous_kart/params/planner.yaml
delete mode 100644 install/autonomous_kart/share/autonomous_kart/params/safety.yaml
delete mode 100644 install/autonomous_kart/share/colcon-core/packages/autonomous_kart
delete mode 100644 install/local_setup.bash
delete mode 100644 install/local_setup.ps1
delete mode 100644 install/local_setup.sh
delete mode 100644 install/local_setup.zsh
delete mode 100644 install/setup.bash
delete mode 100644 install/setup.ps1
delete mode 100644 install/setup.sh
delete mode 100644 install/setup.zsh
delete mode 100644 log/COLCON_IGNORE
delete mode 120000 log/latest
delete mode 120000 log/latest_build
diff --git a/install/.colcon_install_layout b/install/.colcon_install_layout
deleted file mode 100644
index 3aad5336..00000000
--- a/install/.colcon_install_layout
+++ /dev/null
@@ -1 +0,0 @@
-isolated
diff --git a/install/COLCON_IGNORE b/install/COLCON_IGNORE
deleted file mode 100644
index e69de29b..00000000
diff --git a/install/_local_setup_util_ps1.py b/install/_local_setup_util_ps1.py
deleted file mode 100644
index 3c6d9e87..00000000
--- a/install/_local_setup_util_ps1.py
+++ /dev/null
@@ -1,407 +0,0 @@
-# Copyright 2016-2019 Dirk Thomas
-# Licensed under the Apache License, Version 2.0
-
-import argparse
-from collections import OrderedDict
-import os
-from pathlib import Path
-import sys
-
-
-FORMAT_STR_COMMENT_LINE = '# {comment}'
-FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"'
-FORMAT_STR_USE_ENV_VAR = '$env:{name}'
-FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' # noqa: E501
-FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' # noqa: E501
-FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' # noqa: E501
-
-DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate'
-DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate'
-DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists'
-DSV_TYPE_SET = 'set'
-DSV_TYPE_SET_IF_UNSET = 'set-if-unset'
-DSV_TYPE_SOURCE = 'source'
-
-
-def main(argv=sys.argv[1:]): # noqa: D103
- parser = argparse.ArgumentParser(
- description='Output shell commands for the packages in topological '
- 'order')
- parser.add_argument(
- 'primary_extension',
- help='The file extension of the primary shell')
- parser.add_argument(
- 'additional_extension', nargs='?',
- help='The additional file extension to be considered')
- parser.add_argument(
- '--merged-install', action='store_true',
- help='All install prefixes are merged into a single location')
- args = parser.parse_args(argv)
-
- packages = get_packages(Path(__file__).parent, args.merged_install)
-
- ordered_packages = order_packages(packages)
- for pkg_name in ordered_packages:
- if _include_comments():
- print(
- FORMAT_STR_COMMENT_LINE.format_map(
- {'comment': 'Package: ' + pkg_name}))
- prefix = os.path.abspath(os.path.dirname(__file__))
- if not args.merged_install:
- prefix = os.path.join(prefix, pkg_name)
- for line in get_commands(
- pkg_name, prefix, args.primary_extension,
- args.additional_extension
- ):
- print(line)
-
- for line in _remove_ending_separators():
- print(line)
-
-
-def get_packages(prefix_path, merged_install):
- """
- Find packages based on colcon-specific files created during installation.
-
- :param Path prefix_path: The install prefix path of all packages
- :param bool merged_install: The flag if the packages are all installed
- directly in the prefix or if each package is installed in a subdirectory
- named after the package
- :returns: A mapping from the package name to the set of runtime
- dependencies
- :rtype: dict
- """
- packages = {}
- # since importing colcon_core isn't feasible here the following constant
- # must match colcon_core.location.get_relative_package_index_path()
- subdirectory = 'share/colcon-core/packages'
- if merged_install:
- # return if workspace is empty
- if not (prefix_path / subdirectory).is_dir():
- return packages
- # find all files in the subdirectory
- for p in (prefix_path / subdirectory).iterdir():
- if not p.is_file():
- continue
- if p.name.startswith('.'):
- continue
- add_package_runtime_dependencies(p, packages)
- else:
- # for each subdirectory look for the package specific file
- for p in prefix_path.iterdir():
- if not p.is_dir():
- continue
- if p.name.startswith('.'):
- continue
- p = p / subdirectory / p.name
- if p.is_file():
- add_package_runtime_dependencies(p, packages)
-
- # remove unknown dependencies
- pkg_names = set(packages.keys())
- for k in packages.keys():
- packages[k] = {d for d in packages[k] if d in pkg_names}
-
- return packages
-
-
-def add_package_runtime_dependencies(path, packages):
- """
- Check the path and if it exists extract the packages runtime dependencies.
-
- :param Path path: The resource file containing the runtime dependencies
- :param dict packages: A mapping from package names to the sets of runtime
- dependencies to add to
- """
- content = path.read_text()
- dependencies = set(content.split(os.pathsep) if content else [])
- packages[path.name] = dependencies
-
-
-def order_packages(packages):
- """
- Order packages topologically.
-
- :param dict packages: A mapping from package name to the set of runtime
- dependencies
- :returns: The package names
- :rtype: list
- """
- # select packages with no dependencies in alphabetical order
- to_be_ordered = list(packages.keys())
- ordered = []
- while to_be_ordered:
- pkg_names_without_deps = [
- name for name in to_be_ordered if not packages[name]]
- if not pkg_names_without_deps:
- reduce_cycle_set(packages)
- raise RuntimeError(
- 'Circular dependency between: ' + ', '.join(sorted(packages)))
- pkg_names_without_deps.sort()
- pkg_name = pkg_names_without_deps[0]
- to_be_ordered.remove(pkg_name)
- ordered.append(pkg_name)
- # remove item from dependency lists
- for k in list(packages.keys()):
- if pkg_name in packages[k]:
- packages[k].remove(pkg_name)
- return ordered
-
-
-def reduce_cycle_set(packages):
- """
- Reduce the set of packages to the ones part of the circular dependency.
-
- :param dict packages: A mapping from package name to the set of runtime
- dependencies which is modified in place
- """
- last_depended = None
- while len(packages) > 0:
- # get all remaining dependencies
- depended = set()
- for pkg_name, dependencies in packages.items():
- depended = depended.union(dependencies)
- # remove all packages which are not dependent on
- for name in list(packages.keys()):
- if name not in depended:
- del packages[name]
- if last_depended:
- # if remaining packages haven't changed return them
- if last_depended == depended:
- return packages.keys()
- # otherwise reduce again
- last_depended = depended
-
-
-def _include_comments():
- # skipping comment lines when COLCON_TRACE is not set speeds up the
- # processing especially on Windows
- return bool(os.environ.get('COLCON_TRACE'))
-
-
-def get_commands(pkg_name, prefix, primary_extension, additional_extension):
- commands = []
- package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv')
- if os.path.exists(package_dsv_path):
- commands += process_dsv_file(
- package_dsv_path, prefix, primary_extension, additional_extension)
- return commands
-
-
-def process_dsv_file(
- dsv_path, prefix, primary_extension=None, additional_extension=None
-):
- commands = []
- if _include_comments():
- commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path}))
- with open(dsv_path, 'r') as h:
- content = h.read()
- lines = content.splitlines()
-
- basenames = OrderedDict()
- for i, line in enumerate(lines):
- # skip over empty or whitespace-only lines
- if not line.strip():
- continue
- # skip over comments
- if line.startswith('#'):
- continue
- try:
- type_, remainder = line.split(';', 1)
- except ValueError:
- raise RuntimeError(
- "Line %d in '%s' doesn't contain a semicolon separating the "
- 'type from the arguments' % (i + 1, dsv_path))
- if type_ != DSV_TYPE_SOURCE:
- # handle non-source lines
- try:
- commands += handle_dsv_types_except_source(
- type_, remainder, prefix)
- except RuntimeError as e:
- raise RuntimeError(
- "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e
- else:
- # group remaining source lines by basename
- path_without_ext, ext = os.path.splitext(remainder)
- if path_without_ext not in basenames:
- basenames[path_without_ext] = set()
- assert ext.startswith('.')
- ext = ext[1:]
- if ext in (primary_extension, additional_extension):
- basenames[path_without_ext].add(ext)
-
- # add the dsv extension to each basename if the file exists
- for basename, extensions in basenames.items():
- if not os.path.isabs(basename):
- basename = os.path.join(prefix, basename)
- if os.path.exists(basename + '.dsv'):
- extensions.add('dsv')
-
- for basename, extensions in basenames.items():
- if not os.path.isabs(basename):
- basename = os.path.join(prefix, basename)
- if 'dsv' in extensions:
- # process dsv files recursively
- commands += process_dsv_file(
- basename + '.dsv', prefix, primary_extension=primary_extension,
- additional_extension=additional_extension)
- elif primary_extension in extensions and len(extensions) == 1:
- # source primary-only files
- commands += [
- FORMAT_STR_INVOKE_SCRIPT.format_map({
- 'prefix': prefix,
- 'script_path': basename + '.' + primary_extension})]
- elif additional_extension in extensions:
- # source non-primary files
- commands += [
- FORMAT_STR_INVOKE_SCRIPT.format_map({
- 'prefix': prefix,
- 'script_path': basename + '.' + additional_extension})]
-
- return commands
-
-
-def handle_dsv_types_except_source(type_, remainder, prefix):
- commands = []
- if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET):
- try:
- env_name, value = remainder.split(';', 1)
- except ValueError:
- raise RuntimeError(
- "doesn't contain a semicolon separating the environment name "
- 'from the value')
- try_prefixed_value = os.path.join(prefix, value) if value else prefix
- if os.path.exists(try_prefixed_value):
- value = try_prefixed_value
- if type_ == DSV_TYPE_SET:
- commands += _set(env_name, value)
- elif type_ == DSV_TYPE_SET_IF_UNSET:
- commands += _set_if_unset(env_name, value)
- else:
- assert False
- elif type_ in (
- DSV_TYPE_APPEND_NON_DUPLICATE,
- DSV_TYPE_PREPEND_NON_DUPLICATE,
- DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS
- ):
- try:
- env_name_and_values = remainder.split(';')
- except ValueError:
- raise RuntimeError(
- "doesn't contain a semicolon separating the environment name "
- 'from the values')
- env_name = env_name_and_values[0]
- values = env_name_and_values[1:]
- for value in values:
- if not value:
- value = prefix
- elif not os.path.isabs(value):
- value = os.path.join(prefix, value)
- if (
- type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and
- not os.path.exists(value)
- ):
- comment = f'skip extending {env_name} with not existing ' \
- f'path: {value}'
- if _include_comments():
- commands.append(
- FORMAT_STR_COMMENT_LINE.format_map({'comment': comment}))
- elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE:
- commands += _append_unique_value(env_name, value)
- else:
- commands += _prepend_unique_value(env_name, value)
- else:
- raise RuntimeError(
- 'contains an unknown environment hook type: ' + type_)
- return commands
-
-
-env_state = {}
-
-
-def _append_unique_value(name, value):
- global env_state
- if name not in env_state:
- if os.environ.get(name):
- env_state[name] = set(os.environ[name].split(os.pathsep))
- else:
- env_state[name] = set()
- # append even if the variable has not been set yet, in case a shell script sets the
- # same variable without the knowledge of this Python script.
- # later _remove_ending_separators() will cleanup any unintentional leading separator
- extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep
- line = FORMAT_STR_SET_ENV_VAR.format_map(
- {'name': name, 'value': extend + value})
- if value not in env_state[name]:
- env_state[name].add(value)
- else:
- if not _include_comments():
- return []
- line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
- return [line]
-
-
-def _prepend_unique_value(name, value):
- global env_state
- if name not in env_state:
- if os.environ.get(name):
- env_state[name] = set(os.environ[name].split(os.pathsep))
- else:
- env_state[name] = set()
- # prepend even if the variable has not been set yet, in case a shell script sets the
- # same variable without the knowledge of this Python script.
- # later _remove_ending_separators() will cleanup any unintentional trailing separator
- extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name})
- line = FORMAT_STR_SET_ENV_VAR.format_map(
- {'name': name, 'value': value + extend})
- if value not in env_state[name]:
- env_state[name].add(value)
- else:
- if not _include_comments():
- return []
- line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
- return [line]
-
-
-# generate commands for removing prepended underscores
-def _remove_ending_separators():
- # do nothing if the shell extension does not implement the logic
- if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None:
- return []
-
- global env_state
- commands = []
- for name in env_state:
- # skip variables that already had values before this script started prepending
- if name in os.environ:
- continue
- commands += [
- FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}),
- FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})]
- return commands
-
-
-def _set(name, value):
- global env_state
- env_state[name] = value
- line = FORMAT_STR_SET_ENV_VAR.format_map(
- {'name': name, 'value': value})
- return [line]
-
-
-def _set_if_unset(name, value):
- global env_state
- line = FORMAT_STR_SET_ENV_VAR.format_map(
- {'name': name, 'value': value})
- if env_state.get(name, os.environ.get(name)):
- line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
- return [line]
-
-
-if __name__ == '__main__': # pragma: no cover
- try:
- rc = main()
- except RuntimeError as e:
- print(str(e), file=sys.stderr)
- rc = 1
- sys.exit(rc)
diff --git a/install/_local_setup_util_sh.py b/install/_local_setup_util_sh.py
deleted file mode 100644
index f67eaa98..00000000
--- a/install/_local_setup_util_sh.py
+++ /dev/null
@@ -1,407 +0,0 @@
-# Copyright 2016-2019 Dirk Thomas
-# Licensed under the Apache License, Version 2.0
-
-import argparse
-from collections import OrderedDict
-import os
-from pathlib import Path
-import sys
-
-
-FORMAT_STR_COMMENT_LINE = '# {comment}'
-FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"'
-FORMAT_STR_USE_ENV_VAR = '${name}'
-FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' # noqa: E501
-FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501
-FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501
-
-DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate'
-DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate'
-DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists'
-DSV_TYPE_SET = 'set'
-DSV_TYPE_SET_IF_UNSET = 'set-if-unset'
-DSV_TYPE_SOURCE = 'source'
-
-
-def main(argv=sys.argv[1:]): # noqa: D103
- parser = argparse.ArgumentParser(
- description='Output shell commands for the packages in topological '
- 'order')
- parser.add_argument(
- 'primary_extension',
- help='The file extension of the primary shell')
- parser.add_argument(
- 'additional_extension', nargs='?',
- help='The additional file extension to be considered')
- parser.add_argument(
- '--merged-install', action='store_true',
- help='All install prefixes are merged into a single location')
- args = parser.parse_args(argv)
-
- packages = get_packages(Path(__file__).parent, args.merged_install)
-
- ordered_packages = order_packages(packages)
- for pkg_name in ordered_packages:
- if _include_comments():
- print(
- FORMAT_STR_COMMENT_LINE.format_map(
- {'comment': 'Package: ' + pkg_name}))
- prefix = os.path.abspath(os.path.dirname(__file__))
- if not args.merged_install:
- prefix = os.path.join(prefix, pkg_name)
- for line in get_commands(
- pkg_name, prefix, args.primary_extension,
- args.additional_extension
- ):
- print(line)
-
- for line in _remove_ending_separators():
- print(line)
-
-
-def get_packages(prefix_path, merged_install):
- """
- Find packages based on colcon-specific files created during installation.
-
- :param Path prefix_path: The install prefix path of all packages
- :param bool merged_install: The flag if the packages are all installed
- directly in the prefix or if each package is installed in a subdirectory
- named after the package
- :returns: A mapping from the package name to the set of runtime
- dependencies
- :rtype: dict
- """
- packages = {}
- # since importing colcon_core isn't feasible here the following constant
- # must match colcon_core.location.get_relative_package_index_path()
- subdirectory = 'share/colcon-core/packages'
- if merged_install:
- # return if workspace is empty
- if not (prefix_path / subdirectory).is_dir():
- return packages
- # find all files in the subdirectory
- for p in (prefix_path / subdirectory).iterdir():
- if not p.is_file():
- continue
- if p.name.startswith('.'):
- continue
- add_package_runtime_dependencies(p, packages)
- else:
- # for each subdirectory look for the package specific file
- for p in prefix_path.iterdir():
- if not p.is_dir():
- continue
- if p.name.startswith('.'):
- continue
- p = p / subdirectory / p.name
- if p.is_file():
- add_package_runtime_dependencies(p, packages)
-
- # remove unknown dependencies
- pkg_names = set(packages.keys())
- for k in packages.keys():
- packages[k] = {d for d in packages[k] if d in pkg_names}
-
- return packages
-
-
-def add_package_runtime_dependencies(path, packages):
- """
- Check the path and if it exists extract the packages runtime dependencies.
-
- :param Path path: The resource file containing the runtime dependencies
- :param dict packages: A mapping from package names to the sets of runtime
- dependencies to add to
- """
- content = path.read_text()
- dependencies = set(content.split(os.pathsep) if content else [])
- packages[path.name] = dependencies
-
-
-def order_packages(packages):
- """
- Order packages topologically.
-
- :param dict packages: A mapping from package name to the set of runtime
- dependencies
- :returns: The package names
- :rtype: list
- """
- # select packages with no dependencies in alphabetical order
- to_be_ordered = list(packages.keys())
- ordered = []
- while to_be_ordered:
- pkg_names_without_deps = [
- name for name in to_be_ordered if not packages[name]]
- if not pkg_names_without_deps:
- reduce_cycle_set(packages)
- raise RuntimeError(
- 'Circular dependency between: ' + ', '.join(sorted(packages)))
- pkg_names_without_deps.sort()
- pkg_name = pkg_names_without_deps[0]
- to_be_ordered.remove(pkg_name)
- ordered.append(pkg_name)
- # remove item from dependency lists
- for k in list(packages.keys()):
- if pkg_name in packages[k]:
- packages[k].remove(pkg_name)
- return ordered
-
-
-def reduce_cycle_set(packages):
- """
- Reduce the set of packages to the ones part of the circular dependency.
-
- :param dict packages: A mapping from package name to the set of runtime
- dependencies which is modified in place
- """
- last_depended = None
- while len(packages) > 0:
- # get all remaining dependencies
- depended = set()
- for pkg_name, dependencies in packages.items():
- depended = depended.union(dependencies)
- # remove all packages which are not dependent on
- for name in list(packages.keys()):
- if name not in depended:
- del packages[name]
- if last_depended:
- # if remaining packages haven't changed return them
- if last_depended == depended:
- return packages.keys()
- # otherwise reduce again
- last_depended = depended
-
-
-def _include_comments():
- # skipping comment lines when COLCON_TRACE is not set speeds up the
- # processing especially on Windows
- return bool(os.environ.get('COLCON_TRACE'))
-
-
-def get_commands(pkg_name, prefix, primary_extension, additional_extension):
- commands = []
- package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv')
- if os.path.exists(package_dsv_path):
- commands += process_dsv_file(
- package_dsv_path, prefix, primary_extension, additional_extension)
- return commands
-
-
-def process_dsv_file(
- dsv_path, prefix, primary_extension=None, additional_extension=None
-):
- commands = []
- if _include_comments():
- commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path}))
- with open(dsv_path, 'r') as h:
- content = h.read()
- lines = content.splitlines()
-
- basenames = OrderedDict()
- for i, line in enumerate(lines):
- # skip over empty or whitespace-only lines
- if not line.strip():
- continue
- # skip over comments
- if line.startswith('#'):
- continue
- try:
- type_, remainder = line.split(';', 1)
- except ValueError:
- raise RuntimeError(
- "Line %d in '%s' doesn't contain a semicolon separating the "
- 'type from the arguments' % (i + 1, dsv_path))
- if type_ != DSV_TYPE_SOURCE:
- # handle non-source lines
- try:
- commands += handle_dsv_types_except_source(
- type_, remainder, prefix)
- except RuntimeError as e:
- raise RuntimeError(
- "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e
- else:
- # group remaining source lines by basename
- path_without_ext, ext = os.path.splitext(remainder)
- if path_without_ext not in basenames:
- basenames[path_without_ext] = set()
- assert ext.startswith('.')
- ext = ext[1:]
- if ext in (primary_extension, additional_extension):
- basenames[path_without_ext].add(ext)
-
- # add the dsv extension to each basename if the file exists
- for basename, extensions in basenames.items():
- if not os.path.isabs(basename):
- basename = os.path.join(prefix, basename)
- if os.path.exists(basename + '.dsv'):
- extensions.add('dsv')
-
- for basename, extensions in basenames.items():
- if not os.path.isabs(basename):
- basename = os.path.join(prefix, basename)
- if 'dsv' in extensions:
- # process dsv files recursively
- commands += process_dsv_file(
- basename + '.dsv', prefix, primary_extension=primary_extension,
- additional_extension=additional_extension)
- elif primary_extension in extensions and len(extensions) == 1:
- # source primary-only files
- commands += [
- FORMAT_STR_INVOKE_SCRIPT.format_map({
- 'prefix': prefix,
- 'script_path': basename + '.' + primary_extension})]
- elif additional_extension in extensions:
- # source non-primary files
- commands += [
- FORMAT_STR_INVOKE_SCRIPT.format_map({
- 'prefix': prefix,
- 'script_path': basename + '.' + additional_extension})]
-
- return commands
-
-
-def handle_dsv_types_except_source(type_, remainder, prefix):
- commands = []
- if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET):
- try:
- env_name, value = remainder.split(';', 1)
- except ValueError:
- raise RuntimeError(
- "doesn't contain a semicolon separating the environment name "
- 'from the value')
- try_prefixed_value = os.path.join(prefix, value) if value else prefix
- if os.path.exists(try_prefixed_value):
- value = try_prefixed_value
- if type_ == DSV_TYPE_SET:
- commands += _set(env_name, value)
- elif type_ == DSV_TYPE_SET_IF_UNSET:
- commands += _set_if_unset(env_name, value)
- else:
- assert False
- elif type_ in (
- DSV_TYPE_APPEND_NON_DUPLICATE,
- DSV_TYPE_PREPEND_NON_DUPLICATE,
- DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS
- ):
- try:
- env_name_and_values = remainder.split(';')
- except ValueError:
- raise RuntimeError(
- "doesn't contain a semicolon separating the environment name "
- 'from the values')
- env_name = env_name_and_values[0]
- values = env_name_and_values[1:]
- for value in values:
- if not value:
- value = prefix
- elif not os.path.isabs(value):
- value = os.path.join(prefix, value)
- if (
- type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and
- not os.path.exists(value)
- ):
- comment = f'skip extending {env_name} with not existing ' \
- f'path: {value}'
- if _include_comments():
- commands.append(
- FORMAT_STR_COMMENT_LINE.format_map({'comment': comment}))
- elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE:
- commands += _append_unique_value(env_name, value)
- else:
- commands += _prepend_unique_value(env_name, value)
- else:
- raise RuntimeError(
- 'contains an unknown environment hook type: ' + type_)
- return commands
-
-
-env_state = {}
-
-
-def _append_unique_value(name, value):
- global env_state
- if name not in env_state:
- if os.environ.get(name):
- env_state[name] = set(os.environ[name].split(os.pathsep))
- else:
- env_state[name] = set()
- # append even if the variable has not been set yet, in case a shell script sets the
- # same variable without the knowledge of this Python script.
- # later _remove_ending_separators() will cleanup any unintentional leading separator
- extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep
- line = FORMAT_STR_SET_ENV_VAR.format_map(
- {'name': name, 'value': extend + value})
- if value not in env_state[name]:
- env_state[name].add(value)
- else:
- if not _include_comments():
- return []
- line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
- return [line]
-
-
-def _prepend_unique_value(name, value):
- global env_state
- if name not in env_state:
- if os.environ.get(name):
- env_state[name] = set(os.environ[name].split(os.pathsep))
- else:
- env_state[name] = set()
- # prepend even if the variable has not been set yet, in case a shell script sets the
- # same variable without the knowledge of this Python script.
- # later _remove_ending_separators() will cleanup any unintentional trailing separator
- extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name})
- line = FORMAT_STR_SET_ENV_VAR.format_map(
- {'name': name, 'value': value + extend})
- if value not in env_state[name]:
- env_state[name].add(value)
- else:
- if not _include_comments():
- return []
- line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
- return [line]
-
-
-# generate commands for removing prepended underscores
-def _remove_ending_separators():
- # do nothing if the shell extension does not implement the logic
- if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None:
- return []
-
- global env_state
- commands = []
- for name in env_state:
- # skip variables that already had values before this script started prepending
- if name in os.environ:
- continue
- commands += [
- FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}),
- FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})]
- return commands
-
-
-def _set(name, value):
- global env_state
- env_state[name] = value
- line = FORMAT_STR_SET_ENV_VAR.format_map(
- {'name': name, 'value': value})
- return [line]
-
-
-def _set_if_unset(name, value):
- global env_state
- line = FORMAT_STR_SET_ENV_VAR.format_map(
- {'name': name, 'value': value})
- if env_state.get(name, os.environ.get(name)):
- line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
- return [line]
-
-
-if __name__ == '__main__': # pragma: no cover
- try:
- rc = main()
- except RuntimeError as e:
- print(str(e), file=sys.stderr)
- rc = 1
- sys.exit(rc)
diff --git a/install/autonomous_kart/share/ament_index/resource_index/packages/autonomous_kart b/install/autonomous_kart/share/ament_index/resource_index/packages/autonomous_kart
deleted file mode 100644
index e69de29b..00000000
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.dsv b/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.dsv
deleted file mode 100644
index 79d4c95b..00000000
--- a/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.dsv
+++ /dev/null
@@ -1 +0,0 @@
-prepend-non-duplicate;AMENT_PREFIX_PATH;
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.ps1 b/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.ps1
deleted file mode 100644
index 26b99975..00000000
--- a/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.ps1
+++ /dev/null
@@ -1,3 +0,0 @@
-# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
-
-colcon_prepend_unique_value AMENT_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX"
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.sh b/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.sh
deleted file mode 100644
index f3041f68..00000000
--- a/install/autonomous_kart/share/autonomous_kart/hook/ament_prefix_path.sh
+++ /dev/null
@@ -1,3 +0,0 @@
-# generated from colcon_core/shell/template/hook_prepend_value.sh.em
-
-_colcon_prepend_unique_value AMENT_PREFIX_PATH "$COLCON_CURRENT_PREFIX"
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.dsv b/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.dsv
deleted file mode 100644
index 257067d4..00000000
--- a/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.dsv
+++ /dev/null
@@ -1 +0,0 @@
-prepend-non-duplicate;PYTHONPATH;lib/python3.10/site-packages
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.ps1 b/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.ps1
deleted file mode 100644
index caffe83f..00000000
--- a/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.ps1
+++ /dev/null
@@ -1,3 +0,0 @@
-# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
-
-colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX\lib/python3.10/site-packages"
diff --git a/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.sh b/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.sh
deleted file mode 100644
index 660c3483..00000000
--- a/install/autonomous_kart/share/autonomous_kart/hook/pythonpath.sh
+++ /dev/null
@@ -1,3 +0,0 @@
-# generated from colcon_core/shell/template/hook_prepend_value.sh.em
-
-_colcon_prepend_unique_value PYTHONPATH "$COLCON_CURRENT_PREFIX/lib/python3.10/site-packages"
diff --git a/install/autonomous_kart/share/autonomous_kart/launch/bringup_pi.launch.py b/install/autonomous_kart/share/autonomous_kart/launch/bringup_pi.launch.py
deleted file mode 100644
index 88a2aff8..00000000
--- a/install/autonomous_kart/share/autonomous_kart/launch/bringup_pi.launch.py
+++ /dev/null
@@ -1,49 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-import os
-from ament_index_python.packages import get_package_share_directory
-
-
-def generate_launch_description():
- pkg_share = get_package_share_directory('autonomous_kart')
-
- return LaunchDescription([
- Node(
- package='autonomous_kart',
- executable='motor_node',
- name='motor_node',
- parameters=[os.path.join(pkg_share, 'params', 'controller.yaml'), {'simulation_mode': False}]
- ),
- Node(
- package='autonomous_kart',
- executable='steering_node',
- name='steering_node',
- parameters=[os.path.join(pkg_share, 'params', 'controller.yaml')]
- ),
- Node(
- package='autonomous_kart',
- executable='camera_node',
- name='camera_node',
- parameters=[os.path.join(pkg_share, 'params', 'camera.yaml')]
- ),
- Node(
- package='autonomous_kart',
- executable='gps_node',
- name='gps_node',
- parameters=[os.path.join(pkg_share, 'params', 'gps.yaml')]
- ),
- Node(
- package='autonomous_kart',
- executable='pathfinder_node',
- name='pathfinder_node',
- parameters=[os.path.join(pkg_share, 'params', 'planner.yaml'),
- os.path.join(pkg_share, 'params', 'safety.yaml'), os.path.join(pkg_share, 'params', 'gps.yaml')]
- ),
- Node(
- package='autonomous_kart',
- executable='opencv_pathfinder_node',
- name='opencv_pathfinder_node',
- # parameters=[os.path.join(pkg_share, 'params', 'planner.yaml'),
- # os.path.join(pkg_share, 'params', 'safety.yaml'), os.path.join(pkg_share, 'params', 'gps.yaml')]
- ),
- ])
diff --git a/install/autonomous_kart/share/autonomous_kart/launch/bringup_sim.launch.py b/install/autonomous_kart/share/autonomous_kart/launch/bringup_sim.launch.py
deleted file mode 100644
index 3e8f0212..00000000
--- a/install/autonomous_kart/share/autonomous_kart/launch/bringup_sim.launch.py
+++ /dev/null
@@ -1,45 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-import os
-from ament_index_python.packages import get_package_share_directory
-
-
-def generate_launch_description():
- pkg_share = get_package_share_directory('autonomous_kart')
-
- return LaunchDescription([
- Node(
- package='autonomous_kart',
- executable='motor_node',
- name='motor_node',
- parameters=[os.path.join(pkg_share, 'params', 'controller.yaml'), {'simulation_mode': True}]
- ),
- Node(
- package='autonomous_kart',
- executable='steering_node',
- name='steering_node',
- parameters=[os.path.join(pkg_share, 'params', 'controller.yaml'), {'simulation_mode': True}]
- ),
- Node(
- package='autonomous_kart',
- executable='camera_node',
- name='camera_node',
- parameters=[os.path.join(pkg_share, 'params', 'camera.yaml'), {'simulation_mode': True}]
- ),
- Node(
- package='autonomous_kart',
- executable='pathfinder_node',
- name='pathfinder_node',
- parameters=[os.path.join(pkg_share, 'params', 'planner.yaml'),
- os.path.join(pkg_share, 'params', 'safety.yaml'), os.path.join(pkg_share, 'params', 'gps.yaml'),
- {'simulation_mode': True}]
- ),
- Node(
- package='autonomous_kart',
- executable='opencv_pathfinder_node',
- name='opencv_pathfinder_node',
- parameters=[os.path.join(pkg_share, 'params', 'planner.yaml'),
- os.path.join(pkg_share, 'params', 'safety.yaml'), os.path.join(pkg_share, 'params', 'gps.yaml'),
- {'simulation_mode': True}]
- ),
- ])
diff --git a/install/autonomous_kart/share/autonomous_kart/package.bash b/install/autonomous_kart/share/autonomous_kart/package.bash
deleted file mode 100644
index 48271952..00000000
--- a/install/autonomous_kart/share/autonomous_kart/package.bash
+++ /dev/null
@@ -1,31 +0,0 @@
-# generated from colcon_bash/shell/template/package.bash.em
-
-# This script extends the environment for this package.
-
-# a bash script is able to determine its own path if necessary
-if [ -z "$COLCON_CURRENT_PREFIX" ]; then
- # the prefix is two levels up from the package specific share directory
- _colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)"
-else
- _colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
-fi
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-# additional arguments: arguments to the script
-_colcon_package_bash_source_script() {
- if [ -f "$1" ]; then
- if [ -n "$COLCON_TRACE" ]; then
- echo "# . \"$1\""
- fi
- . "$@"
- else
- echo "not found: \"$1\"" 1>&2
- fi
-}
-
-# source sh script of this package
-_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/autonomous_kart/package.sh"
-
-unset _colcon_package_bash_source_script
-unset _colcon_package_bash_COLCON_CURRENT_PREFIX
diff --git a/install/autonomous_kart/share/autonomous_kart/package.dsv b/install/autonomous_kart/share/autonomous_kart/package.dsv
deleted file mode 100644
index b9a0734d..00000000
--- a/install/autonomous_kart/share/autonomous_kart/package.dsv
+++ /dev/null
@@ -1,6 +0,0 @@
-source;share/autonomous_kart/hook/pythonpath.ps1
-source;share/autonomous_kart/hook/pythonpath.dsv
-source;share/autonomous_kart/hook/pythonpath.sh
-source;share/autonomous_kart/hook/ament_prefix_path.ps1
-source;share/autonomous_kart/hook/ament_prefix_path.dsv
-source;share/autonomous_kart/hook/ament_prefix_path.sh
diff --git a/install/autonomous_kart/share/autonomous_kart/package.ps1 b/install/autonomous_kart/share/autonomous_kart/package.ps1
deleted file mode 100644
index 3871215d..00000000
--- a/install/autonomous_kart/share/autonomous_kart/package.ps1
+++ /dev/null
@@ -1,116 +0,0 @@
-# generated from colcon_powershell/shell/template/package.ps1.em
-
-# function to append a value to a variable
-# which uses colons as separators
-# duplicates as well as leading separators are avoided
-# first argument: the name of the result variable
-# second argument: the value to be prepended
-function colcon_append_unique_value {
- param (
- $_listname,
- $_value
- )
-
- # get values from variable
- if (Test-Path Env:$_listname) {
- $_values=(Get-Item env:$_listname).Value
- } else {
- $_values=""
- }
- $_duplicate=""
- # start with no values
- $_all_values=""
- # iterate over existing values in the variable
- if ($_values) {
- $_values.Split(";") | ForEach {
- # not an empty string
- if ($_) {
- # not a duplicate of _value
- if ($_ -eq $_value) {
- $_duplicate="1"
- }
- if ($_all_values) {
- $_all_values="${_all_values};$_"
- } else {
- $_all_values="$_"
- }
- }
- }
- }
- # append only non-duplicates
- if (!$_duplicate) {
- # avoid leading separator
- if ($_all_values) {
- $_all_values="${_all_values};${_value}"
- } else {
- $_all_values="${_value}"
- }
- }
-
- # export the updated variable
- Set-Item env:\$_listname -Value "$_all_values"
-}
-
-# function to prepend a value to a variable
-# which uses colons as separators
-# duplicates as well as trailing separators are avoided
-# first argument: the name of the result variable
-# second argument: the value to be prepended
-function colcon_prepend_unique_value {
- param (
- $_listname,
- $_value
- )
-
- # get values from variable
- if (Test-Path Env:$_listname) {
- $_values=(Get-Item env:$_listname).Value
- } else {
- $_values=""
- }
- # start with the new value
- $_all_values="$_value"
- # iterate over existing values in the variable
- if ($_values) {
- $_values.Split(";") | ForEach {
- # not an empty string
- if ($_) {
- # not a duplicate of _value
- if ($_ -ne $_value) {
- # keep non-duplicate values
- $_all_values="${_all_values};$_"
- }
- }
- }
- }
- # export the updated variable
- Set-Item env:\$_listname -Value "$_all_values"
-}
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-# additional arguments: arguments to the script
-function colcon_package_source_powershell_script {
- param (
- $_colcon_package_source_powershell_script
- )
- # source script with conditional trace output
- if (Test-Path $_colcon_package_source_powershell_script) {
- if ($env:COLCON_TRACE) {
- echo ". '$_colcon_package_source_powershell_script'"
- }
- . "$_colcon_package_source_powershell_script"
- } else {
- Write-Error "not found: '$_colcon_package_source_powershell_script'"
- }
-}
-
-
-# a powershell script is able to determine its own path
-# the prefix is two levels up from the package specific share directory
-$env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.FullName
-
-colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/autonomous_kart/hook/pythonpath.ps1"
-colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/autonomous_kart/hook/ament_prefix_path.ps1"
-
-Remove-Item Env:\COLCON_CURRENT_PREFIX
diff --git a/install/autonomous_kart/share/autonomous_kart/package.sh b/install/autonomous_kart/share/autonomous_kart/package.sh
deleted file mode 100644
index d53dab10..00000000
--- a/install/autonomous_kart/share/autonomous_kart/package.sh
+++ /dev/null
@@ -1,87 +0,0 @@
-# generated from colcon_core/shell/template/package.sh.em
-
-# This script extends the environment for this package.
-
-# function to prepend a value to a variable
-# which uses colons as separators
-# duplicates as well as trailing separators are avoided
-# first argument: the name of the result variable
-# second argument: the value to be prepended
-_colcon_prepend_unique_value() {
- # arguments
- _listname="$1"
- _value="$2"
-
- # get values from variable
- eval _values=\"\$$_listname\"
- # backup the field separator
- _colcon_prepend_unique_value_IFS=$IFS
- IFS=":"
- # start with the new value
- _all_values="$_value"
- # workaround SH_WORD_SPLIT not being set in zsh
- if [ "$(command -v colcon_zsh_convert_to_array)" ]; then
- colcon_zsh_convert_to_array _values
- fi
- # iterate over existing values in the variable
- for _item in $_values; do
- # ignore empty strings
- if [ -z "$_item" ]; then
- continue
- fi
- # ignore duplicates of _value
- if [ "$_item" = "$_value" ]; then
- continue
- fi
- # keep non-duplicate values
- _all_values="$_all_values:$_item"
- done
- unset _item
- # restore the field separator
- IFS=$_colcon_prepend_unique_value_IFS
- unset _colcon_prepend_unique_value_IFS
- # export the updated variable
- eval export $_listname=\"$_all_values\"
- unset _all_values
- unset _values
-
- unset _value
- unset _listname
-}
-
-# since a plain shell script can't determine its own path when being sourced
-# either use the provided COLCON_CURRENT_PREFIX
-# or fall back to the build time prefix (if it exists)
-_colcon_package_sh_COLCON_CURRENT_PREFIX="/ws/install/autonomous_kart"
-if [ -z "$COLCON_CURRENT_PREFIX" ]; then
- if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then
- echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
- unset _colcon_package_sh_COLCON_CURRENT_PREFIX
- return 1
- fi
- COLCON_CURRENT_PREFIX="$_colcon_package_sh_COLCON_CURRENT_PREFIX"
-fi
-unset _colcon_package_sh_COLCON_CURRENT_PREFIX
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-# additional arguments: arguments to the script
-_colcon_package_sh_source_script() {
- if [ -f "$1" ]; then
- if [ -n "$COLCON_TRACE" ]; then
- echo "# . \"$1\""
- fi
- . "$@"
- else
- echo "not found: \"$1\"" 1>&2
- fi
-}
-
-# source sh hooks
-_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/autonomous_kart/hook/pythonpath.sh"
-_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/autonomous_kart/hook/ament_prefix_path.sh"
-
-unset _colcon_package_sh_source_script
-unset COLCON_CURRENT_PREFIX
-
-# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks
diff --git a/install/autonomous_kart/share/autonomous_kart/package.xml b/install/autonomous_kart/share/autonomous_kart/package.xml
deleted file mode 100644
index 0005e101..00000000
--- a/install/autonomous_kart/share/autonomous_kart/package.xml
+++ /dev/null
@@ -1,23 +0,0 @@
-
-
-
- autonomous_kart
- 0.0.1
- Package containing all nodes for driving in different states.
- root
- Apache-2.0
-
- ament_python
-
- rclpy
- geometry_msgs
-
- ament_copyright
- ament_flake8
- ament_pep257
- python3-pytest
-
-
- ament_python
-
-
\ No newline at end of file
diff --git a/install/autonomous_kart/share/autonomous_kart/package.zsh b/install/autonomous_kart/share/autonomous_kart/package.zsh
deleted file mode 100644
index 179a08f0..00000000
--- a/install/autonomous_kart/share/autonomous_kart/package.zsh
+++ /dev/null
@@ -1,42 +0,0 @@
-# generated from colcon_zsh/shell/template/package.zsh.em
-
-# This script extends the environment for this package.
-
-# a zsh script is able to determine its own path if necessary
-if [ -z "$COLCON_CURRENT_PREFIX" ]; then
- # the prefix is two levels up from the package specific share directory
- _colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)"
-else
- _colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
-fi
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-# additional arguments: arguments to the script
-_colcon_package_zsh_source_script() {
- if [ -f "$1" ]; then
- if [ -n "$COLCON_TRACE" ]; then
- echo "# . \"$1\""
- fi
- . "$@"
- else
- echo "not found: \"$1\"" 1>&2
- fi
-}
-
-# function to convert array-like strings into arrays
-# to workaround SH_WORD_SPLIT not being set
-colcon_zsh_convert_to_array() {
- local _listname=$1
- local _dollar="$"
- local _split="{="
- local _to_array="(\"$_dollar$_split$_listname}\")"
- eval $_listname=$_to_array
-}
-
-# source sh script of this package
-_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/autonomous_kart/package.sh"
-unset convert_zsh_to_array
-
-unset _colcon_package_zsh_source_script
-unset _colcon_package_zsh_COLCON_CURRENT_PREFIX
diff --git a/install/autonomous_kart/share/autonomous_kart/params/camera.yaml b/install/autonomous_kart/share/autonomous_kart/params/camera.yaml
deleted file mode 100644
index 346227a4..00000000
--- a/install/autonomous_kart/share/autonomous_kart/params/camera.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-# Camera config settings
-camera_node:
- ros__parameters:
- simulation_mode: false # set later
- fps: 30.0 # float
\ No newline at end of file
diff --git a/install/autonomous_kart/share/autonomous_kart/params/controller.yaml b/install/autonomous_kart/share/autonomous_kart/params/controller.yaml
deleted file mode 100644
index 3bd8bf1a..00000000
--- a/install/autonomous_kart/share/autonomous_kart/params/controller.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-# Params and toggles for RC controller drive including gain, rates, etc.
-# Dummy values for now
-motor_node:
- ros__parameters:
- max_linear_speed: 5.0 # m/s
- max_angular_speed: 3.0 # rad/s
- motor_timeout: 1.0 # seconds
- simulation_mode: false # set in motor node
\ No newline at end of file
diff --git a/install/autonomous_kart/share/autonomous_kart/params/gps.yaml b/install/autonomous_kart/share/autonomous_kart/params/gps.yaml
deleted file mode 100644
index e69de29b..00000000
diff --git a/install/autonomous_kart/share/autonomous_kart/params/planner.yaml b/install/autonomous_kart/share/autonomous_kart/params/planner.yaml
deleted file mode 100644
index 17ee3bcd..00000000
--- a/install/autonomous_kart/share/autonomous_kart/params/planner.yaml
+++ /dev/null
@@ -1 +0,0 @@
-# Planner configs including perception thresholds, toggles, etc.
\ No newline at end of file
diff --git a/install/autonomous_kart/share/autonomous_kart/params/safety.yaml b/install/autonomous_kart/share/autonomous_kart/params/safety.yaml
deleted file mode 100644
index 39564898..00000000
--- a/install/autonomous_kart/share/autonomous_kart/params/safety.yaml
+++ /dev/null
@@ -1 +0,0 @@
-# Safety limits and estop config
\ No newline at end of file
diff --git a/install/autonomous_kart/share/colcon-core/packages/autonomous_kart b/install/autonomous_kart/share/colcon-core/packages/autonomous_kart
deleted file mode 100644
index 18e15c42..00000000
--- a/install/autonomous_kart/share/colcon-core/packages/autonomous_kart
+++ /dev/null
@@ -1 +0,0 @@
-geometry_msgs:rclpy
\ No newline at end of file
diff --git a/install/local_setup.bash b/install/local_setup.bash
deleted file mode 100644
index 03f00256..00000000
--- a/install/local_setup.bash
+++ /dev/null
@@ -1,121 +0,0 @@
-# generated from colcon_bash/shell/template/prefix.bash.em
-
-# This script extends the environment with all packages contained in this
-# prefix path.
-
-# a bash script is able to determine its own path if necessary
-if [ -z "$COLCON_CURRENT_PREFIX" ]; then
- _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)"
-else
- _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
-fi
-
-# function to prepend a value to a variable
-# which uses colons as separators
-# duplicates as well as trailing separators are avoided
-# first argument: the name of the result variable
-# second argument: the value to be prepended
-_colcon_prefix_bash_prepend_unique_value() {
- # arguments
- _listname="$1"
- _value="$2"
-
- # get values from variable
- eval _values=\"\$$_listname\"
- # backup the field separator
- _colcon_prefix_bash_prepend_unique_value_IFS="$IFS"
- IFS=":"
- # start with the new value
- _all_values="$_value"
- _contained_value=""
- # iterate over existing values in the variable
- for _item in $_values; do
- # ignore empty strings
- if [ -z "$_item" ]; then
- continue
- fi
- # ignore duplicates of _value
- if [ "$_item" = "$_value" ]; then
- _contained_value=1
- continue
- fi
- # keep non-duplicate values
- _all_values="$_all_values:$_item"
- done
- unset _item
- if [ -z "$_contained_value" ]; then
- if [ -n "$COLCON_TRACE" ]; then
- if [ "$_all_values" = "$_value" ]; then
- echo "export $_listname=$_value"
- else
- echo "export $_listname=$_value:\$$_listname"
- fi
- fi
- fi
- unset _contained_value
- # restore the field separator
- IFS="$_colcon_prefix_bash_prepend_unique_value_IFS"
- unset _colcon_prefix_bash_prepend_unique_value_IFS
- # export the updated variable
- eval export $_listname=\"$_all_values\"
- unset _all_values
- unset _values
-
- unset _value
- unset _listname
-}
-
-# add this prefix to the COLCON_PREFIX_PATH
-_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX"
-unset _colcon_prefix_bash_prepend_unique_value
-
-# check environment variable for custom Python executable
-if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then
- if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then
- echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist"
- return 1
- fi
- _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE"
-else
- # try the Python executable known at configure time
- _colcon_python_executable="/usr/bin/python3"
- # if it doesn't exist try a fall back
- if [ ! -f "$_colcon_python_executable" ]; then
- if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then
- echo "error: unable to find python3 executable"
- return 1
- fi
- _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"`
- fi
-fi
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-_colcon_prefix_sh_source_script() {
- if [ -f "$1" ]; then
- if [ -n "$COLCON_TRACE" ]; then
- echo "# . \"$1\""
- fi
- . "$1"
- else
- echo "not found: \"$1\"" 1>&2
- fi
-}
-
-# get all commands in topological order
-_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)"
-unset _colcon_python_executable
-if [ -n "$COLCON_TRACE" ]; then
- echo "$(declare -f _colcon_prefix_sh_source_script)"
- echo "# Execute generated script:"
- echo "# <<<"
- echo "${_colcon_ordered_commands}"
- echo "# >>>"
- echo "unset _colcon_prefix_sh_source_script"
-fi
-eval "${_colcon_ordered_commands}"
-unset _colcon_ordered_commands
-
-unset _colcon_prefix_sh_source_script
-
-unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX
diff --git a/install/local_setup.ps1 b/install/local_setup.ps1
deleted file mode 100644
index 6f68c8de..00000000
--- a/install/local_setup.ps1
+++ /dev/null
@@ -1,55 +0,0 @@
-# generated from colcon_powershell/shell/template/prefix.ps1.em
-
-# This script extends the environment with all packages contained in this
-# prefix path.
-
-# check environment variable for custom Python executable
-if ($env:COLCON_PYTHON_EXECUTABLE) {
- if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) {
- echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist"
- exit 1
- }
- $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE"
-} else {
- # use the Python executable known at configure time
- $_colcon_python_executable="/usr/bin/python3"
- # if it doesn't exist try a fall back
- if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) {
- if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) {
- echo "error: unable to find python3 executable"
- exit 1
- }
- $_colcon_python_executable="python3"
- }
-}
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-function _colcon_prefix_powershell_source_script {
- param (
- $_colcon_prefix_powershell_source_script_param
- )
- # source script with conditional trace output
- if (Test-Path $_colcon_prefix_powershell_source_script_param) {
- if ($env:COLCON_TRACE) {
- echo ". '$_colcon_prefix_powershell_source_script_param'"
- }
- . "$_colcon_prefix_powershell_source_script_param"
- } else {
- Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'"
- }
-}
-
-# get all commands in topological order
-$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1
-
-# execute all commands in topological order
-if ($env:COLCON_TRACE) {
- echo "Execute generated script:"
- echo "<<<"
- $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output
- echo ">>>"
-}
-if ($_colcon_ordered_commands) {
- $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression
-}
diff --git a/install/local_setup.sh b/install/local_setup.sh
deleted file mode 100644
index 6ddd1a7f..00000000
--- a/install/local_setup.sh
+++ /dev/null
@@ -1,137 +0,0 @@
-# generated from colcon_core/shell/template/prefix.sh.em
-
-# This script extends the environment with all packages contained in this
-# prefix path.
-
-# since a plain shell script can't determine its own path when being sourced
-# either use the provided COLCON_CURRENT_PREFIX
-# or fall back to the build time prefix (if it exists)
-_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/ws/install"
-if [ -z "$COLCON_CURRENT_PREFIX" ]; then
- if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then
- echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
- unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX
- return 1
- fi
-else
- _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
-fi
-
-# function to prepend a value to a variable
-# which uses colons as separators
-# duplicates as well as trailing separators are avoided
-# first argument: the name of the result variable
-# second argument: the value to be prepended
-_colcon_prefix_sh_prepend_unique_value() {
- # arguments
- _listname="$1"
- _value="$2"
-
- # get values from variable
- eval _values=\"\$$_listname\"
- # backup the field separator
- _colcon_prefix_sh_prepend_unique_value_IFS="$IFS"
- IFS=":"
- # start with the new value
- _all_values="$_value"
- _contained_value=""
- # iterate over existing values in the variable
- for _item in $_values; do
- # ignore empty strings
- if [ -z "$_item" ]; then
- continue
- fi
- # ignore duplicates of _value
- if [ "$_item" = "$_value" ]; then
- _contained_value=1
- continue
- fi
- # keep non-duplicate values
- _all_values="$_all_values:$_item"
- done
- unset _item
- if [ -z "$_contained_value" ]; then
- if [ -n "$COLCON_TRACE" ]; then
- if [ "$_all_values" = "$_value" ]; then
- echo "export $_listname=$_value"
- else
- echo "export $_listname=$_value:\$$_listname"
- fi
- fi
- fi
- unset _contained_value
- # restore the field separator
- IFS="$_colcon_prefix_sh_prepend_unique_value_IFS"
- unset _colcon_prefix_sh_prepend_unique_value_IFS
- # export the updated variable
- eval export $_listname=\"$_all_values\"
- unset _all_values
- unset _values
-
- unset _value
- unset _listname
-}
-
-# add this prefix to the COLCON_PREFIX_PATH
-_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX"
-unset _colcon_prefix_sh_prepend_unique_value
-
-# check environment variable for custom Python executable
-if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then
- if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then
- echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist"
- return 1
- fi
- _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE"
-else
- # try the Python executable known at configure time
- _colcon_python_executable="/usr/bin/python3"
- # if it doesn't exist try a fall back
- if [ ! -f "$_colcon_python_executable" ]; then
- if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then
- echo "error: unable to find python3 executable"
- return 1
- fi
- _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"`
- fi
-fi
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-_colcon_prefix_sh_source_script() {
- if [ -f "$1" ]; then
- if [ -n "$COLCON_TRACE" ]; then
- echo "# . \"$1\""
- fi
- . "$1"
- else
- echo "not found: \"$1\"" 1>&2
- fi
-}
-
-# get all commands in topological order
-_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)"
-unset _colcon_python_executable
-if [ -n "$COLCON_TRACE" ]; then
- echo "_colcon_prefix_sh_source_script() {
- if [ -f \"\$1\" ]; then
- if [ -n \"\$COLCON_TRACE\" ]; then
- echo \"# . \\\"\$1\\\"\"
- fi
- . \"\$1\"
- else
- echo \"not found: \\\"\$1\\\"\" 1>&2
- fi
- }"
- echo "# Execute generated script:"
- echo "# <<<"
- echo "${_colcon_ordered_commands}"
- echo "# >>>"
- echo "unset _colcon_prefix_sh_source_script"
-fi
-eval "${_colcon_ordered_commands}"
-unset _colcon_ordered_commands
-
-unset _colcon_prefix_sh_source_script
-
-unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX
diff --git a/install/local_setup.zsh b/install/local_setup.zsh
deleted file mode 100644
index b6487102..00000000
--- a/install/local_setup.zsh
+++ /dev/null
@@ -1,134 +0,0 @@
-# generated from colcon_zsh/shell/template/prefix.zsh.em
-
-# This script extends the environment with all packages contained in this
-# prefix path.
-
-# a zsh script is able to determine its own path if necessary
-if [ -z "$COLCON_CURRENT_PREFIX" ]; then
- _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)"
-else
- _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
-fi
-
-# function to convert array-like strings into arrays
-# to workaround SH_WORD_SPLIT not being set
-_colcon_prefix_zsh_convert_to_array() {
- local _listname=$1
- local _dollar="$"
- local _split="{="
- local _to_array="(\"$_dollar$_split$_listname}\")"
- eval $_listname=$_to_array
-}
-
-# function to prepend a value to a variable
-# which uses colons as separators
-# duplicates as well as trailing separators are avoided
-# first argument: the name of the result variable
-# second argument: the value to be prepended
-_colcon_prefix_zsh_prepend_unique_value() {
- # arguments
- _listname="$1"
- _value="$2"
-
- # get values from variable
- eval _values=\"\$$_listname\"
- # backup the field separator
- _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS"
- IFS=":"
- # start with the new value
- _all_values="$_value"
- _contained_value=""
- # workaround SH_WORD_SPLIT not being set
- _colcon_prefix_zsh_convert_to_array _values
- # iterate over existing values in the variable
- for _item in $_values; do
- # ignore empty strings
- if [ -z "$_item" ]; then
- continue
- fi
- # ignore duplicates of _value
- if [ "$_item" = "$_value" ]; then
- _contained_value=1
- continue
- fi
- # keep non-duplicate values
- _all_values="$_all_values:$_item"
- done
- unset _item
- if [ -z "$_contained_value" ]; then
- if [ -n "$COLCON_TRACE" ]; then
- if [ "$_all_values" = "$_value" ]; then
- echo "export $_listname=$_value"
- else
- echo "export $_listname=$_value:\$$_listname"
- fi
- fi
- fi
- unset _contained_value
- # restore the field separator
- IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS"
- unset _colcon_prefix_zsh_prepend_unique_value_IFS
- # export the updated variable
- eval export $_listname=\"$_all_values\"
- unset _all_values
- unset _values
-
- unset _value
- unset _listname
-}
-
-# add this prefix to the COLCON_PREFIX_PATH
-_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX"
-unset _colcon_prefix_zsh_prepend_unique_value
-unset _colcon_prefix_zsh_convert_to_array
-
-# check environment variable for custom Python executable
-if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then
- if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then
- echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist"
- return 1
- fi
- _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE"
-else
- # try the Python executable known at configure time
- _colcon_python_executable="/usr/bin/python3"
- # if it doesn't exist try a fall back
- if [ ! -f "$_colcon_python_executable" ]; then
- if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then
- echo "error: unable to find python3 executable"
- return 1
- fi
- _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"`
- fi
-fi
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-_colcon_prefix_sh_source_script() {
- if [ -f "$1" ]; then
- if [ -n "$COLCON_TRACE" ]; then
- echo "# . \"$1\""
- fi
- . "$1"
- else
- echo "not found: \"$1\"" 1>&2
- fi
-}
-
-# get all commands in topological order
-_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)"
-unset _colcon_python_executable
-if [ -n "$COLCON_TRACE" ]; then
- echo "$(declare -f _colcon_prefix_sh_source_script)"
- echo "# Execute generated script:"
- echo "# <<<"
- echo "${_colcon_ordered_commands}"
- echo "# >>>"
- echo "unset _colcon_prefix_sh_source_script"
-fi
-eval "${_colcon_ordered_commands}"
-unset _colcon_ordered_commands
-
-unset _colcon_prefix_sh_source_script
-
-unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX
diff --git a/install/setup.bash b/install/setup.bash
deleted file mode 100644
index 10ea0f7c..00000000
--- a/install/setup.bash
+++ /dev/null
@@ -1,31 +0,0 @@
-# generated from colcon_bash/shell/template/prefix_chain.bash.em
-
-# This script extends the environment with the environment of other prefix
-# paths which were sourced when this file was generated as well as all packages
-# contained in this prefix path.
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-_colcon_prefix_chain_bash_source_script() {
- if [ -f "$1" ]; then
- if [ -n "$COLCON_TRACE" ]; then
- echo "# . \"$1\""
- fi
- . "$1"
- else
- echo "not found: \"$1\"" 1>&2
- fi
-}
-
-# source chained prefixes
-# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
-COLCON_CURRENT_PREFIX="/opt/ros/humble"
-_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
-
-# source this prefix
-# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
-COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)"
-_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
-
-unset COLCON_CURRENT_PREFIX
-unset _colcon_prefix_chain_bash_source_script
diff --git a/install/setup.ps1 b/install/setup.ps1
deleted file mode 100644
index 558e9b9e..00000000
--- a/install/setup.ps1
+++ /dev/null
@@ -1,29 +0,0 @@
-# generated from colcon_powershell/shell/template/prefix_chain.ps1.em
-
-# This script extends the environment with the environment of other prefix
-# paths which were sourced when this file was generated as well as all packages
-# contained in this prefix path.
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-function _colcon_prefix_chain_powershell_source_script {
- param (
- $_colcon_prefix_chain_powershell_source_script_param
- )
- # source script with conditional trace output
- if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) {
- if ($env:COLCON_TRACE) {
- echo ". '$_colcon_prefix_chain_powershell_source_script_param'"
- }
- . "$_colcon_prefix_chain_powershell_source_script_param"
- } else {
- Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'"
- }
-}
-
-# source chained prefixes
-_colcon_prefix_chain_powershell_source_script "/opt/ros/humble\local_setup.ps1"
-
-# source this prefix
-$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent)
-_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1"
diff --git a/install/setup.sh b/install/setup.sh
deleted file mode 100644
index 7a978ade..00000000
--- a/install/setup.sh
+++ /dev/null
@@ -1,45 +0,0 @@
-# generated from colcon_core/shell/template/prefix_chain.sh.em
-
-# This script extends the environment with the environment of other prefix
-# paths which were sourced when this file was generated as well as all packages
-# contained in this prefix path.
-
-# since a plain shell script can't determine its own path when being sourced
-# either use the provided COLCON_CURRENT_PREFIX
-# or fall back to the build time prefix (if it exists)
-_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/ws/install
-if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then
- _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
-elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then
- echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
- unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX
- return 1
-fi
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-_colcon_prefix_chain_sh_source_script() {
- if [ -f "$1" ]; then
- if [ -n "$COLCON_TRACE" ]; then
- echo "# . \"$1\""
- fi
- . "$1"
- else
- echo "not found: \"$1\"" 1>&2
- fi
-}
-
-# source chained prefixes
-# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script
-COLCON_CURRENT_PREFIX="/opt/ros/humble"
-_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh"
-
-
-# source this prefix
-# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script
-COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX"
-_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh"
-
-unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX
-unset _colcon_prefix_chain_sh_source_script
-unset COLCON_CURRENT_PREFIX
diff --git a/install/setup.zsh b/install/setup.zsh
deleted file mode 100644
index 54799fde..00000000
--- a/install/setup.zsh
+++ /dev/null
@@ -1,31 +0,0 @@
-# generated from colcon_zsh/shell/template/prefix_chain.zsh.em
-
-# This script extends the environment with the environment of other prefix
-# paths which were sourced when this file was generated as well as all packages
-# contained in this prefix path.
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-_colcon_prefix_chain_zsh_source_script() {
- if [ -f "$1" ]; then
- if [ -n "$COLCON_TRACE" ]; then
- echo "# . \"$1\""
- fi
- . "$1"
- else
- echo "not found: \"$1\"" 1>&2
- fi
-}
-
-# source chained prefixes
-# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
-COLCON_CURRENT_PREFIX="/opt/ros/humble"
-_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh"
-
-# source this prefix
-# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
-COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)"
-_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh"
-
-unset COLCON_CURRENT_PREFIX
-unset _colcon_prefix_chain_zsh_source_script
diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE
deleted file mode 100644
index e69de29b..00000000
diff --git a/log/latest b/log/latest
deleted file mode 120000
index b57d247c..00000000
--- a/log/latest
+++ /dev/null
@@ -1 +0,0 @@
-latest_build
\ No newline at end of file
diff --git a/log/latest_build b/log/latest_build
deleted file mode 120000
index 36f1f9ad..00000000
--- a/log/latest_build
+++ /dev/null
@@ -1 +0,0 @@
-build_2025-10-23_00-39-04
\ No newline at end of file
From bf09c5218ea9ae337e3e59f22862ad07d33f4571 Mon Sep 17 00:00:00 2001
From: shb-png
Date: Wed, 5 Nov 2025 06:22:44 -0500
Subject: [PATCH 12/21] remove old testing files
---
opencv_testing/angle_calculator.py | 184 -----------------------------
opencv_testing/opencv_testing.py | 19 ---
opencv_testing/pathfinder.py | 50 --------
3 files changed, 253 deletions(-)
delete mode 100644 opencv_testing/angle_calculator.py
delete mode 100644 opencv_testing/opencv_testing.py
delete mode 100644 opencv_testing/pathfinder.py
diff --git a/opencv_testing/angle_calculator.py b/opencv_testing/angle_calculator.py
deleted file mode 100644
index 54488c3a..00000000
--- a/opencv_testing/angle_calculator.py
+++ /dev/null
@@ -1,184 +0,0 @@
-"""
-Compute theta_left and theta_right from a video.
-Requirements: opencv-python (cv2), numpy
-Usage: tweak CAMERA_FX or HFOV_HORIZONTAL to match your camera.
-"""
-
-import cv2
-import numpy as np
-import math
-
-# ----------------- CONFIG -----------------
-VIDEO_PATH = "IMG_8824.mp4" # change to your file or use 0 for webcam
-USE_CALIBRATION = False # True if you have fx, cx from calibration
-CAMERA_FX = 800.0 # focal length in pixels (only if USE_CALIBRATION)
-CAMERA_CX = None # principal point x; if None -> image_width/2
-HFOV_DEG = 70.0 # horizontal field of view (deg) if no fx available
-
-SMOOTH_ALPHA = 0.7 # for low-pass filtering of theta
-CANNY_THRESH1 = 50
-CANNY_THRESH2 = 150
-
-# Ray sampling parameters
-MAX_SAMPLE_DIST = 400 # max pixels to scan along each ray
-FORWARD_STEP = 1 # pixels per sample along forward ray
-RIGHT_STEP = 1 # pixels per sample along right ray
-
-# -----------------------------------------
-
-def compute_fx_from_hfov(width, hfov_deg):
- hfov = math.radians(hfov_deg)
- return (width / 2.0) / math.tan(hfov / 2.0)
-
-def pixel_to_angle(u, fx, cx):
- # returns angle in degrees, negative = left of center
- return math.degrees(math.atan2((u - cx), fx))
-
-def find_intersection_along_column(mask, col, start_row, step=1, max_dist=400):
- """
- Scan downwards along column 'col' starting at start_row (row index),
- return (u,v) of first mask nonzero pixel, or None if none found.
- """
- h, w = mask.shape
- row = start_row
- dist = 0
- while dist < max_dist and 0 <= row < h:
- if mask[row, col]:
- return col, row
- row += step
- dist += abs(step)
- return None
-
-def find_intersection_along_row(mask, row, start_col, step=1, max_dist=400):
- """
- Scan rightwards along row 'row' starting at start_col, return first mask hit.
- """
- h, w = mask.shape
- col = start_col
- dist = 0
- while dist < max_dist and 0 <= col < w:
- if mask[row, col]:
- return col, row
- col += step
- dist += abs(step)
- return None
-
-def main():
- cap = cv2.VideoCapture(VIDEO_PATH)
- if not cap.isOpened():
- print("Cannot open video:", VIDEO_PATH)
- return
-
- theta_left_f = None
- theta_right_f = None
-
- while True:
- ret, frame = cap.read()
- if not ret:
- break
-
- h, w = frame.shape[:2]
- cx = CAMERA_CX if CAMERA_CX is not None else w / 2.0
- if USE_CALIBRATION:
- fx = CAMERA_FX
- else:
- fx = compute_fx_from_hfov(w, HFOV_DEG)
-
- # 1) preprocess and boundary mask
- gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
- blur = cv2.GaussianBlur(gray, (5,5), 0)
- edges = cv2.Canny(blur, CANNY_THRESH1, CANNY_THRESH2)
- # optional morphology to fill gaps
- kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5,5))
- mask = cv2.morphologyEx(edges, cv2.MORPH_CLOSE, kernel)
-
- # 2) define rays: use image center
- center_col = int(round(cx))
- center_row = int(round(h / 2))
-
- # forward ray: downwards from center_row toward bottom (increasing row)
- f_hit = find_intersection_along_column(mask, center_col, center_row, step=FORWARD_STEP, max_dist=MAX_SAMPLE_DIST)
-
- # right ray: from center out to the right along center_row
- r_hit = find_intersection_along_row(mask, center_row, center_col, step=RIGHT_STEP, max_dist=MAX_SAMPLE_DIST)
-
- # convert hits to angles (default to None/confidence if not found)
- theta_left = None
- theta_right = None
-
- if f_hit is not None:
- u_f, v_f = f_hit
- theta_forward = pixel_to_angle(u_f, fx, cx) # bearing of forward intersection
- # If the forward ray hits boundary on right side of center, we can treat that as right boundary?
- # However your definition: theta_left is angle from left boundary to center — we approximate:
- # We'll set theta_left to the bearing of the left-side boundary detected forward of center,
- # but since forward ray is center column, use it as a nearby boundary reading if needed.
- # For consistent approach: treat forward ray hit on its x location to compute whichever boundary that represents.
- if u_f < cx:
- theta_left = pixel_to_angle(u_f, fx, cx)
- else:
- theta_right = pixel_to_angle(u_f, fx, cx)
-
- if r_hit is not None:
- u_r, v_r = r_hit
- # point to the right of center -> this is likely the right boundary
- theta_right = pixel_to_angle(u_r, fx, cx)
-
- # If we failed to find one of them from the rays, try alternate strategy:
- # find contours and pick nearest contour point to the ray direction (omitted for brevity),
- # or fallback to last frame value.
-
- # fallback: if missing, reuse previous smoothed value or compute from any contour
- if theta_left is None and theta_left_f is not None:
- theta_left = theta_left_f
- if theta_right is None and theta_right_f is not None:
- theta_right = theta_right_f
-
- # smoothing
- if theta_left is not None:
- theta_left_f = theta_left if theta_left_f is None else (SMOOTH_ALPHA * theta_left_f + (1-SMOOTH_ALPHA)*theta_left)
- if theta_right is not None:
- theta_right_f = theta_right if theta_right_f is None else (SMOOTH_ALPHA * theta_right_f + (1-SMOOTH_ALPHA)*theta_right)
-
- # compute desired heading if both exist (or using whichever exists)
- desired_heading = None
- if (theta_left_f is not None) and (theta_right_f is not None):
- desired_heading = 0.5 * (theta_left_f + theta_right_f)
- elif theta_left_f is not None:
- desired_heading = theta_left_f # single-side fallback
- elif theta_right_f is not None:
- desired_heading = theta_right_f
-
- # Display overlay for debugging
- vis = frame.copy()
- # draw rays
- cv2.line(vis, (center_col, center_row), (center_col, min(h, center_row + MAX_SAMPLE_DIST)), (0,255,0), 1)
- cv2.line(vis, (center_col, center_row), (min(w-1, center_col + MAX_SAMPLE_DIST), center_row), (0,255,0), 1)
- if f_hit is not None:
- cv2.circle(vis, (f_hit[0], f_hit[1]), 6, (0,0,255), -1)
- if r_hit is not None:
- cv2.circle(vis, (r_hit[0], r_hit[1]), 6, (255,0,0), -1)
-
- # text
- info = [
- f"theta_left_f: {theta_left_f:.2f}" if theta_left_f is not None else "theta_left_f: N/A",
- f"theta_right_f: {theta_right_f:.2f}" if theta_right_f is not None else "theta_right_f: N/A",
- f"desired_heading: {desired_heading:.2f}" if desired_heading is not None else "desired_heading: N/A"
- ]
- y = 30
- for line in info:
- cv2.putText(vis, line, (10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,255), 2)
- y += 25
-
- cv2.imshow("vis", vis)
- cv2.imshow("mask", mask)
-
- # press q to quit
- if cv2.waitKey(1) & 0xFF == ord('q'):
- break
-
- cap.release()
- cv2.destroyAllWindows()
-
-if __name__ == "__main__":
- main()
diff --git a/opencv_testing/opencv_testing.py b/opencv_testing/opencv_testing.py
deleted file mode 100644
index dc56c1f4..00000000
--- a/opencv_testing/opencv_testing.py
+++ /dev/null
@@ -1,19 +0,0 @@
-import cv2
-
-cap = cv2.VideoCapture("IMG_8824.mp4")
-
-while True:
- ret, frame = cap.read()
- if not ret:
- break
-
- gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
-
- cv2.imshow("Video", gray_frame) #shows the video
-
- # Press 'q' to exit early
- if cv2.waitKey(25) & 0xFF == ord('q'):
- break
-
-cap.release()
-cv2.destroyAllWindows()
diff --git a/opencv_testing/pathfinder.py b/opencv_testing/pathfinder.py
deleted file mode 100644
index 2e9157ea..00000000
--- a/opencv_testing/pathfinder.py
+++ /dev/null
@@ -1,50 +0,0 @@
-import time
-import os
-
-speed = 0.0 # mph
-max_accel = 3.0 # mph per second
-max_steering = 90 # degrees
-time_step = 1 # seconds
-
-theta1 = -5
-theta2 = 5
-
-for t in range(1, 31):
- if t <= 10:
- theta1 = -5
- theta2 = 5
- elif 10 < t <= 20:
- theta1 -= 2
- theta2 += 0.5
- elif 20 < t <= 30:
- theta1 += 2
- theta2 += 2
-
- desired_heading = (theta1 + theta2) / 2 #average of both angles
-
- if abs(desired_heading) < 10:
- target_speed = 30 #max speed on straights
- else:
- target_speed = 15 #target speed on turns
-
- if speed < target_speed:
- speed += max_accel * time_step #physics c: mechanics
- if speed > target_speed:
- speed = target_speed
- else:
- speed -= max_accel * time_step
- if speed < target_speed:
- speed = target_speed
-
- steering_command = max(min(desired_heading, max_steering), -max_steering)
-
- os.system('cls' if os.name == 'nt' else 'clear')
-
- print(f"Time: {t} sec")
- print(f"Theta1: {theta1:.1f}°, Theta2: {theta2:.1f}°")
- print(f"Desired Heading: {desired_heading:.1f}°")
- print(f"Target Speed: {target_speed} mph | Current Speed: {speed:.1f} mph")
- print(f"Steering Command: {steering_command:.1f}° {'Left' if steering_command < 0 else 'Right' if steering_command > 0 else 'Straight'}")
-
- time.sleep(1)
-
From 3a8dc7a4e6370a35618e30af52703fbfa34c08f9 Mon Sep 17 00:00:00 2001
From: shb-png
Date: Wed, 5 Nov 2025 06:24:27 -0500
Subject: [PATCH 13/21] revert dummy values
---
.../autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py b/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py
index 9d42c74c..90984b83 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py
@@ -10,4 +10,4 @@ def calculate_track_angles(frame: cv2.Mat) -> Tuple[float, float]:
:param frame: Most recent image from camera
:return: Tuple of [left angle (degrees), right angle (degrees)]. For example, return 75.0, 75.0
"""
- return 67.0, 41.0 # dummy
\ No newline at end of file
+ return 75.0, 75.0 # dummy
\ No newline at end of file
From a492e8138223b88285f4a1dbbc5510778f4f9720 Mon Sep 17 00:00:00 2001
From: shb-png
Date: Wed, 5 Nov 2025 06:29:27 -0500
Subject: [PATCH 14/21] re-adding shutdown improvments
for some reason we got rid of it during the merge conflict part idk man
---
.../autonomous_kart/nodes/pathfinder/pathfinder_node.py | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
index 6df6a8c3..be288ee6 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
@@ -88,7 +88,8 @@ def main(args=None):
node.get_logger().error('Unhandled exception', exc_info=True)
finally:
node.destroy_node()
- rclpy.shutdown()
+ if rclpy.ok():
+ rclpy.shutdown()
if __name__ == '__main__':
From 7d9d723bd8207ce747121b74819d6f76aeb1da2c Mon Sep 17 00:00:00 2001
From: shb-png
Date: Wed, 5 Nov 2025 12:08:37 +0000
Subject: [PATCH 15/21] use current speed from motor node instead of hardcoded
value
---
.../nodes/pathfinder/pathfinder.py | 39 +++++++++----------
.../nodes/pathfinder/pathfinder_node.py | 27 +++++++++++--
2 files changed, 42 insertions(+), 24 deletions(-)
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
index 15f14125..8ff5a116 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
@@ -2,12 +2,18 @@
from typing import Tuple
-def pathfinder(opencv_output: Tuple, logger):
+def pathfinder(opencv_output: Tuple, current_speed: float, dt, logger):
+ """
+ Calculate commands for steering and motor from opencv_pathfinder efficiently
+ Part of hot loop so must be efficient.
+ :param opencv_output: Tuple of [left angle from center to base of track from image (float32), right angle ...]
+ :param current_speed: current speed from motor node
+ :return: Returns commands to motor & steering in (speed % of total, steering angle (degrees from -90 to 90 with 0 as straight)
+ """
- speed = 0.0 # mph
max_accel = 3.0 # mph per second
max_steering = 25 # degrees
- time_step = 1 # seconds
+ speed_command = current_speed
theta1 = -1 * opencv_output[0]
theta2 = opencv_output[1]
@@ -19,29 +25,20 @@ def pathfinder(opencv_output: Tuple, logger):
else:
target_speed = 15 #target speed on turns
- if speed < target_speed:
- speed += max_accel * time_step #physics c: mechanics
- if speed > target_speed:
- speed = target_speed
+ if current_speed < target_speed:
+ speed_command += max_accel * dt #physics c: mechanics
+ if speed_command > target_speed:
+ speed_command = target_speed
else:
- speed -= max_accel * time_step
- if speed < target_speed:
- speed = target_speed
+ speed_command -= max_accel * dt
+ if speed_command < target_speed:
+ speed_command = target_speed
steering_command = max(min(desired_heading, max_steering), -max_steering)
logger.info(f"Theta1: {theta1:.1f}°, Theta2: {theta2:.1f}°")
logger.info(f"Desired Heading: {desired_heading:.1f}°")
- logger.info(f"Target Speed: {target_speed} mph | Current Speed: {speed:.1f} mph")
+ logger.info(f"Target Speed: {target_speed} mph | Current Speed: {current_speed:.1f} mph | Commanded Speed: {speed_command:.1f} mph")
logger.info(f"Steering Command: {steering_command:.1f}° {'Left' if steering_command < 0 else 'Right' if steering_command > 0 else 'Straight'}")
-
- """
- Calculate commands for steering and motor from opencv_pathfinder efficiently
- Part of hot loop so must be efficient.
- TODO(Pathfinder Team): Calculate motor_speed & steering_angle
- :param opencv_output: Tuple of [left angle from center to base of track from image (float32), right angle ...]
- :return: Returns commands to motor & steering in (speed % of total, steering angle (degrees from -90 to 90 with 0 as straight)
- """
-
- return float(speed), float(steering_command)
+ return float(speed_command), float(steering_command)
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
index be288ee6..af8fddf0 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
@@ -10,9 +10,11 @@ def __init__(self):
super().__init__('PathfinderNode')
self.logger = self.get_logger()
self.angles = None
+ self.current_speed = 0.0
self.cmd_count = 0
self.last_log_time = self.get_clock().now()
+ self.last_path_time = self.get_clock().now()
self.declare_parameter('system_frequency', 60)
self.system_frequency = self.get_parameter('system_frequency').value
@@ -28,6 +30,14 @@ def __init__(self):
5
)
+ # Subscriber to motor node for current speed
+ self.motor_speed_subscriber = self.create_subscription(
+ Float32,
+ 'motor_speed',
+ self.motor_speed_callback,
+ 5
+ )
+
# Publisher to motor
self.motor_publisher = self.create_publisher(
Float32,
@@ -35,7 +45,7 @@ def __init__(self):
5
)
- # # Publisher to steering
+ # Publisher to steering
self.steering_publisher = self.create_publisher(
Float32,
'cmd_turn',
@@ -54,11 +64,22 @@ def calculate_path_callback(self, msg: Float32MultiArray):
self.cmd_count += 1
self.angles = (msg.data[0], msg.data[1])
- motor_speed, steering_angle = pathfinder(msg.data, self.logger)
+ current_time = self.get_clock().now()
+ dt = (current_time - self.last_path_time).nanoseconds / 1e9
+
+ self.last_path_time = self.get_clock().now()
+
+ motor_speed, steering_angle = pathfinder(msg.data, self.current_speed, dt, self.logger)
self.steering_publisher.publish(Float32(data=steering_angle))
self.motor_publisher.publish(Float32(data=motor_speed))
-
+
+ def motor_speed_callback(self, msg: Float32):
+ """
+ Updates current speed from motor feedback (required for smooth acceleration??)
+ :param msg: Float32 containing current motor speed
+ """
+ self.current_speed = msg.data
def log_command_rate(self):
"""Log average commands per second every 5 seconds"""
current_time = self.get_clock().now()
From f26f98785cd85df946b02219c90569d6241b5d1b Mon Sep 17 00:00:00 2001
From: shb-png
Date: Wed, 5 Nov 2025 12:50:50 +0000
Subject: [PATCH 16/21] make speed command % of total speed
---
.../nodes/pathfinder/pathfinder.py | 18 +++++++++++++++---
1 file changed, 15 insertions(+), 3 deletions(-)
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
index 8ff5a116..4694e102 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
@@ -13,6 +13,13 @@ def pathfinder(opencv_output: Tuple, current_speed: float, dt, logger):
max_accel = 3.0 # mph per second
max_steering = 25 # degrees
+ max_speed_straight = 30.0 # max speed on straights (mph)
+ max_speed_turning = 15.0 # max speed while completing turns (mph)
+ max_speed = 35.0 # max speed car is capable of going (mph) (for calculating speed % of total)
+
+ # convert current speed from motor node (presumably also a % of total?) to mph for calculations
+ current_speed *= max_speed
+
speed_command = current_speed
theta1 = -1 * opencv_output[0]
@@ -21,9 +28,9 @@ def pathfinder(opencv_output: Tuple, current_speed: float, dt, logger):
desired_heading = (theta1 + theta2) / 2 #average of both angles
if abs(desired_heading) < 10:
- target_speed = 30 #max speed on straights
+ target_speed = max_speed_straight #max speed on straights
else:
- target_speed = 15 #target speed on turns
+ target_speed = max_speed_turning #target speed on turns
if current_speed < target_speed:
speed_command += max_accel * dt #physics c: mechanics
@@ -36,9 +43,14 @@ def pathfinder(opencv_output: Tuple, current_speed: float, dt, logger):
steering_command = max(min(desired_heading, max_steering), -max_steering)
+ # calculate speed command as % of total
+ speed_command /= max_speed
+
logger.info(f"Theta1: {theta1:.1f}°, Theta2: {theta2:.1f}°")
logger.info(f"Desired Heading: {desired_heading:.1f}°")
- logger.info(f"Target Speed: {target_speed} mph | Current Speed: {current_speed:.1f} mph | Commanded Speed: {speed_command:.1f} mph")
+ logger.info(f"Target Speed: {target_speed} mph | Current Speed: {current_speed:.1f} mph | Commanded Speed: {speed_command:.3f} of total")
logger.info(f"Steering Command: {steering_command:.1f}° {'Left' if steering_command < 0 else 'Right' if steering_command > 0 else 'Straight'}")
+
+
return float(speed_command), float(steering_command)
From 9400330610821e8df87bc311b224dafe55c332d1 Mon Sep 17 00:00:00 2001
From: shb-png
Date: Wed, 5 Nov 2025 15:12:23 +0000
Subject: [PATCH 17/21] move pathfinding parameters to controller.yaml
---
.../nodes/pathfinder/pathfinder.py | 13 +++-------
.../nodes/pathfinder/pathfinder_node.py | 26 ++++++++++++++++++-
.../autonomous_kart/params/controller.yaml | 4 +++
3 files changed, 33 insertions(+), 10 deletions(-)
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
index 4694e102..35575071 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
@@ -1,22 +1,16 @@
-import random
from typing import Tuple
-
-def pathfinder(opencv_output: Tuple, current_speed: float, dt, logger):
+def pathfinder(opencv_output: Tuple, current_speed: float, dt, max_accel, max_steering, max_speed_straight, max_speed_turning, max_speed, logger):
"""
Calculate commands for steering and motor from opencv_pathfinder efficiently
Part of hot loop so must be efficient.
:param opencv_output: Tuple of [left angle from center to base of track from image (float32), right angle ...]
:param current_speed: current speed from motor node
+ :param dt: time between last calculated speed command and current
+ :param logger: allows for logging info
:return: Returns commands to motor & steering in (speed % of total, steering angle (degrees from -90 to 90 with 0 as straight)
"""
- max_accel = 3.0 # mph per second
- max_steering = 25 # degrees
- max_speed_straight = 30.0 # max speed on straights (mph)
- max_speed_turning = 15.0 # max speed while completing turns (mph)
- max_speed = 35.0 # max speed car is capable of going (mph) (for calculating speed % of total)
-
# convert current speed from motor node (presumably also a % of total?) to mph for calculations
current_speed *= max_speed
@@ -50,6 +44,7 @@ def pathfinder(opencv_output: Tuple, current_speed: float, dt, logger):
logger.info(f"Desired Heading: {desired_heading:.1f}°")
logger.info(f"Target Speed: {target_speed} mph | Current Speed: {current_speed:.1f} mph | Commanded Speed: {speed_command:.3f} of total")
logger.info(f"Steering Command: {steering_command:.1f}° {'Left' if steering_command < 0 else 'Right' if steering_command > 0 else 'Straight'}")
+ logger.info(f"dt {dt:.3f}")
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
index af8fddf0..dea45af5 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
@@ -16,6 +16,20 @@ def __init__(self):
self.last_log_time = self.get_clock().now()
self.last_path_time = self.get_clock().now()
+ # declare parameters w/ defaults (for pathfinding calculations)
+ self.declare_parameter('max_accel', 3.0)
+ self.declare_parameter('max_steering', 25.0)
+ self.declare_parameter('max_speed_straight', 30.0)
+ self.declare_parameter('max_speed_turning', 15.0)
+ self.declare_parameter('max_speed', 35.0)
+
+ # get parameter values required for pathfinding calculations
+ self.max_accel = self.get_parameter('max_accel').value
+ self.max_steering = self.get_parameter('max_steering').value
+ self.max_speed_straight = self.get_parameter('max_speed_straight').value
+ self.max_speed_turning = self.get_parameter('max_speed_turning').value
+ self.max_speed = self.get_parameter('max_speed').value
+
self.declare_parameter('system_frequency', 60)
self.system_frequency = self.get_parameter('system_frequency').value
@@ -69,7 +83,16 @@ def calculate_path_callback(self, msg: Float32MultiArray):
self.last_path_time = self.get_clock().now()
- motor_speed, steering_angle = pathfinder(msg.data, self.current_speed, dt, self.logger)
+ motor_speed, steering_angle = pathfinder(
+ msg.data,
+ self.current_speed,
+ dt,
+ self.max_accel,
+ self.max_steering,
+ self.max_speed_straight,
+ self.max_speed_turning,
+ self.max_speed,
+ self.logger)
self.steering_publisher.publish(Float32(data=steering_angle))
self.motor_publisher.publish(Float32(data=motor_speed))
@@ -80,6 +103,7 @@ def motor_speed_callback(self, msg: Float32):
:param msg: Float32 containing current motor speed
"""
self.current_speed = msg.data
+
def log_command_rate(self):
"""Log average commands per second every 5 seconds"""
current_time = self.get_clock().now()
diff --git a/src/autonomous_kart/autonomous_kart/params/controller.yaml b/src/autonomous_kart/autonomous_kart/params/controller.yaml
index 3bd8bf1a..09009fc4 100644
--- a/src/autonomous_kart/autonomous_kart/params/controller.yaml
+++ b/src/autonomous_kart/autonomous_kart/params/controller.yaml
@@ -5,4 +5,8 @@ motor_node:
max_linear_speed: 5.0 # m/s
max_angular_speed: 3.0 # rad/s
motor_timeout: 1.0 # seconds
+ max_accel: 3.0 # m/s^2
+ max_steering: 25 # degrees
+ max_speed_straight: 30.0 # m/s | max speed on straights
+ max_speed_turning: 15.0 # m/s | speed while completing turns
simulation_mode: false # set in motor node
\ No newline at end of file
From 90f701f9b64e520b62b081a59954987d24f4988d Mon Sep 17 00:00:00 2001
From: shb-png
Date: Wed, 5 Nov 2025 15:23:02 +0000
Subject: [PATCH 18/21] update type hints for all parameters
---
.../nodes/pathfinder/pathfinder.py | 16 +++++++++++-----
1 file changed, 11 insertions(+), 5 deletions(-)
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
index 35575071..12bdefde 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
@@ -1,17 +1,23 @@
from typing import Tuple
+from std_msgs.msg import Float32
-def pathfinder(opencv_output: Tuple, current_speed: float, dt, max_accel, max_steering, max_speed_straight, max_speed_turning, max_speed, logger):
+def pathfinder(opencv_output: Tuple, current_speed: Float32, dt: Float32, max_accel:Float32, max_steering: Float32, max_speed_straight: Float32, max_speed_turning: Float32, max_speed: Float32, logger):
"""
Calculate commands for steering and motor from opencv_pathfinder efficiently
Part of hot loop so must be efficient.
:param opencv_output: Tuple of [left angle from center to base of track from image (float32), right angle ...]
- :param current_speed: current speed from motor node
- :param dt: time between last calculated speed command and current
- :param logger: allows for logging info
+ :param current_speed: Current actual speed from motor node (% of total)
+ :param dt: Time elapsed time between last calculated speed command and current
+ :param max_accel: Max allowed acceleration of kart (m/s)
+ :param max_steering: Max steering angle (degrees)
+ :param max_speed_straight: Max allowed speed on straights (m/s)
+ :param max_speed_turning: Max allowed speed while turning (m/s)
+ :param max_speed: Max possible speed (m/s)
+ :param logger: Allows for logging info
:return: Returns commands to motor & steering in (speed % of total, steering angle (degrees from -90 to 90 with 0 as straight)
"""
- # convert current speed from motor node (presumably also a % of total?) to mph for calculations
+ # convert current speed from motor node to m/s
current_speed *= max_speed
speed_command = current_speed
From d3559a6cb6a7179111f1f891ce54e4d0258b7330 Mon Sep 17 00:00:00 2001
From: shb-png
Date: Wed, 5 Nov 2025 15:42:28 +0000
Subject: [PATCH 19/21] update units in logs
---
.../autonomous_kart/nodes/pathfinder/pathfinder.py | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
index 12bdefde..5874d8ab 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
@@ -48,7 +48,7 @@ def pathfinder(opencv_output: Tuple, current_speed: Float32, dt: Float32, max_ac
logger.info(f"Theta1: {theta1:.1f}°, Theta2: {theta2:.1f}°")
logger.info(f"Desired Heading: {desired_heading:.1f}°")
- logger.info(f"Target Speed: {target_speed} mph | Current Speed: {current_speed:.1f} mph | Commanded Speed: {speed_command:.3f} of total")
+ logger.info(f"Target Speed: {target_speed} m/s | Current Speed: {current_speed:.1f} m/s | Commanded Speed: {speed_command:.3f} of total")
logger.info(f"Steering Command: {steering_command:.1f}° {'Left' if steering_command < 0 else 'Right' if steering_command > 0 else 'Straight'}")
logger.info(f"dt {dt:.3f}")
From 25c5ec9237efb54ae8b2dcf3322f80ce390b0e1c Mon Sep 17 00:00:00 2001
From: shb-png
Date: Wed, 5 Nov 2025 15:53:04 +0000
Subject: [PATCH 20/21] cleaner speed command calc + clearer variable naming
---
.../nodes/pathfinder/pathfinder.py | 18 +++++++-----------
1 file changed, 7 insertions(+), 11 deletions(-)
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
index 5874d8ab..acb2bcf3 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder.py
@@ -20,7 +20,7 @@ def pathfinder(opencv_output: Tuple, current_speed: Float32, dt: Float32, max_ac
# convert current speed from motor node to m/s
current_speed *= max_speed
- speed_command = current_speed
+ speed_command_ms = current_speed # base speed command in m/s
theta1 = -1 * opencv_output[0]
theta2 = opencv_output[1]
@@ -32,26 +32,22 @@ def pathfinder(opencv_output: Tuple, current_speed: Float32, dt: Float32, max_ac
else:
target_speed = max_speed_turning #target speed on turns
- if current_speed < target_speed:
- speed_command += max_accel * dt #physics c: mechanics
- if speed_command > target_speed:
- speed_command = target_speed
+ if target_speed > current_speed:
+ speed_command_ms = min(speed_command_ms + max_accel * dt, target_speed)
else:
- speed_command -= max_accel * dt
- if speed_command < target_speed:
- speed_command = target_speed
+ speed_command_ms = max(speed_command_ms - max_accel * dt, target_speed)
steering_command = max(min(desired_heading, max_steering), -max_steering)
# calculate speed command as % of total
- speed_command /= max_speed
+ speed_command_percent = speed_command_ms / max_speed
logger.info(f"Theta1: {theta1:.1f}°, Theta2: {theta2:.1f}°")
logger.info(f"Desired Heading: {desired_heading:.1f}°")
- logger.info(f"Target Speed: {target_speed} m/s | Current Speed: {current_speed:.1f} m/s | Commanded Speed: {speed_command:.3f} of total")
+ logger.info(f"Target Speed: {target_speed} m/s | Current Speed: {current_speed:.1f} m/s | Commanded Speed: {speed_command_percent:.3f} of total")
logger.info(f"Steering Command: {steering_command:.1f}° {'Left' if steering_command < 0 else 'Right' if steering_command > 0 else 'Straight'}")
logger.info(f"dt {dt:.3f}")
- return float(speed_command), float(steering_command)
+ return float(speed_command_percent), float(steering_command)
From 123de20fd8126391c43854c2425b252082874f6b Mon Sep 17 00:00:00 2001
From: shb-png
Date: Wed, 5 Nov 2025 18:01:31 +0000
Subject: [PATCH 21/21] handle no angles recieved from opnecv
---
.../opencv_pathfinder/angle_calculator.py | 28 ++++++++++-
.../nodes/pathfinder/pathfinder_node.py | 46 +++++++++++++++++--
.../autonomous_kart/params/controller.yaml | 1 +
3 files changed, 71 insertions(+), 4 deletions(-)
diff --git a/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py b/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py
index 90984b83..c0728ddd 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/opencv_pathfinder/angle_calculator.py
@@ -1,6 +1,8 @@
from typing import Tuple
import cv2
+import time
+_start_time = None
def calculate_track_angles(frame: cv2.Mat) -> Tuple[float, float]:
"""
@@ -10,4 +12,28 @@ def calculate_track_angles(frame: cv2.Mat) -> Tuple[float, float]:
:param frame: Most recent image from camera
:return: Tuple of [left angle (degrees), right angle (degrees)]. For example, return 75.0, 75.0
"""
- return 75.0, 75.0 # dummy
\ No newline at end of file
+
+ '''
+ DUMMY CODE FOR SIMULATING OPENCV FAILURE BELOW
+ '''
+ global _start_time
+
+ if _start_time is None:
+ _start_time = time.time()
+
+ elapsed = time.time() - _start_time
+
+ # normal angles for first 5 seconds
+ if elapsed < 5.0:
+ # variation for realism!!
+ left_angle = 70.0 + (elapsed * 2)
+ right_angle = 80.0 - (elapsed * 1)
+ return left_angle, right_angle
+ # return infinity (simulating detection failure)
+ else:
+ return float('inf'), float('inf')
+
+ '''
+ OG DUMMY CODE:
+ return 75.0, 75.0 # dummy
+ '''
\ No newline at end of file
diff --git a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
index dea45af5..7de8f495 100644
--- a/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
+++ b/src/autonomous_kart/autonomous_kart/nodes/pathfinder/pathfinder_node.py
@@ -1,9 +1,8 @@
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32MultiArray, Float32
-
from autonomous_kart.nodes.pathfinder.pathfinder import pathfinder
-
+import numpy as np
class PathfinderNode(Node):
def __init__(self):
@@ -22,6 +21,7 @@ def __init__(self):
self.declare_parameter('max_speed_straight', 30.0)
self.declare_parameter('max_speed_turning', 15.0)
self.declare_parameter('max_speed', 35.0)
+ self.declare_parameter('max_angle_age', 0.5) # max acceptable age of saved angle (s)
# get parameter values required for pathfinding calculations
self.max_accel = self.get_parameter('max_accel').value
@@ -29,6 +29,7 @@ def __init__(self):
self.max_speed_straight = self.get_parameter('max_speed_straight').value
self.max_speed_turning = self.get_parameter('max_speed_turning').value
self.max_speed = self.get_parameter('max_speed').value
+ self.max_angle_age = self.get_parameter('max_angle_age').value
self.declare_parameter('system_frequency', 60)
self.system_frequency = self.get_parameter('system_frequency').value
@@ -83,8 +84,47 @@ def calculate_path_callback(self, msg: Float32MultiArray):
self.last_path_time = self.get_clock().now()
+ # check if passed in angles are valid
+ theta1 = msg.data[0]
+ theta2 = msg.data[1]
+
+ angles_valid = not (np.isinf(theta1) or np.isinf(theta2) or
+ theta1 is None or theta2 is None)
+
+ if angles_valid:
+ # store valid angle w/ timestamp
+ self.last_valid_angles = (theta1, theta2)
+ self.last_valid_timestamp = current_time
+ angles_to_use = (theta1, theta2)
+ self.logger.debug("Using fresh angles from OpenCV")
+ else:
+ # try to use last valid angles
+ self.logger.warn("OpenCV failed to detect angles")
+
+ if self.last_valid_angles is not None and self.last_valid_timestamp is not None:
+ # check age of last saved angle
+ angle_age = (current_time - self.last_valid_timestamp).nanoseconds / 1e9
+
+ if angle_age <= self.max_angle_age:
+ angles_to_use = self.last_valid_angles
+ self.logger.info(f"Using cached angles ({angle_age:.3f}s old)")
+ else:
+ # saved angle too old -- slow down
+ self.logger.warn(f"Saved angle too old ({angle_age:.3f}s > {self.max_angle_age}s) - slowing down | Current command {self.current_speed:.3f}")
+ slowdown_speed = self.current_speed * 0.7 # reduce current speed 30%
+ self.motor_publisher.publish(Float32(data=slowdown_speed))
+ self.steering_publisher.publish(Float32(data=0.0)) # go straight
+ return
+ else:
+ # no valid angles saved -- also slow down
+ self.logger.warn(f"No valid angles available - slowing down | Current command {self.current_speed:.3f}")
+ slowdown_speed = self.current_speed * 0.7
+ self.motor_publisher.publish(Float32(data=slowdown_speed))
+ self.steering_publisher.publish(Float32(data=0.0))
+ return
+
motor_speed, steering_angle = pathfinder(
- msg.data,
+ angles_to_use,
self.current_speed,
dt,
self.max_accel,
diff --git a/src/autonomous_kart/autonomous_kart/params/controller.yaml b/src/autonomous_kart/autonomous_kart/params/controller.yaml
index 09009fc4..68b25d58 100644
--- a/src/autonomous_kart/autonomous_kart/params/controller.yaml
+++ b/src/autonomous_kart/autonomous_kart/params/controller.yaml
@@ -9,4 +9,5 @@ motor_node:
max_steering: 25 # degrees
max_speed_straight: 30.0 # m/s | max speed on straights
max_speed_turning: 15.0 # m/s | speed while completing turns
+ max_angle_age: 0.5 # max acceptable age of saved angle (s)
simulation_mode: false # set in motor node
\ No newline at end of file