diff --git a/.gitignore b/.gitignore index f0ca32f6..e7d1b9db 100644 --- a/.gitignore +++ b/.gitignore @@ -50,7 +50,6 @@ coverage.xml .hypothesis/ .pytest_cache/ cover/ - # Translations *.mo *.pot @@ -176,4 +175,7 @@ cython_debug/ # Ros build ignored install log -build \ No newline at end of file +build + +.vscode/ +.DS_Store \ No newline at end of file diff --git a/data/EVC_test_footage/vid.mp4 b/data/EVC_test_footage/vid.mp4 new file mode 100644 index 00000000..7856691e Binary files /dev/null and b/data/EVC_test_footage/vid.mp4 differ diff --git a/scripts/start.bash b/scripts/start.bash index 93be447f..e9047bfc 100755 --- a/scripts/start.bash +++ b/scripts/start.bash @@ -1,9 +1,8 @@ #!/usr/bin/env bash source /opt/ros/humble/setup.bash +colcon build --symlink-install [ -f /ws/install/setup.bash ] && source /ws/install/setup.bash [ -f /ws/.venv/bin/activate ] && source /ws/.venv/bin/activate -colcon build -pkill -f master_api || true -sleep 0.5 \ No newline at end of file +ros2 launch autonomous_kart bringup_sim.launch.py diff --git a/src/autonomous_kart/autonomous_kart/launch/bringup_pi.launch.py b/src/autonomous_kart/autonomous_kart/launch/bringup_pi.launch.py index 01c01d74..8f204049 100644 --- a/src/autonomous_kart/autonomous_kart/launch/bringup_pi.launch.py +++ b/src/autonomous_kart/autonomous_kart/launch/bringup_pi.launch.py @@ -1,12 +1,14 @@ from launch import LaunchDescription -from launch.actions import GroupAction +from launch.actions import GroupAction, ExecuteProcess from launch_ros.actions import Node, SetParameter, SetParametersFromFile -from ament_index_python.packages import get_package_share_directory +from ament_index_python.packages import get_package_share_directory, get_package_prefix +from pathlib import Path import os def generate_launch_description(): pkg_share = get_package_share_directory("autonomous_kart") + camera_binary = str(Path(pkg_share).resolve().parents[2] / "autonomous_kart_cpp" / "lib" / "autonomous_kart_cpp" / "camera_node") motor_yaml = os.path.join(pkg_share, "params", "motor.yaml") steering_yaml = os.path.join(pkg_share, "params", "steering.yaml") @@ -37,7 +39,7 @@ def generate_launch_description(): parameters=[steering_yaml], ), Node( - package="autonomous_kart", + package="autonomous_kart_cpp", executable="camera_node", name="camera_node", parameters=[camera_yaml], 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 fb5c898c..a2ab0f5a 100644 --- a/src/autonomous_kart/autonomous_kart/launch/bringup_sim.launch.py +++ b/src/autonomous_kart/autonomous_kart/launch/bringup_sim.launch.py @@ -1,13 +1,39 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction +from launch.actions import DeclareLaunchArgument, GroupAction, ExecuteProcess from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node, SetParameter, SetParametersFromFile -from ament_index_python.packages import get_package_share_directory +from ament_index_python.packages import get_package_share_directory, get_package_prefix +from pathlib import Path import os +def _get_camera_binary(pkg_share: str) -> str: + """Resolve camera binary path. + + Prefer the installed prefix for `autonomous_kart_cpp` if available; + fall back to deriving the sibling install layout relative to `pkg_share`. + """ + try: + prefix = get_package_prefix("autonomous_kart_cpp") + candidate = Path(prefix) / "lib" / "autonomous_kart_cpp" / "camera_node" + if candidate.exists(): + return str(candidate) + except Exception: + pass + + # fallback derived path (works in typical overlay layout) + return str( + Path(pkg_share).resolve().parents[2] + / "autonomous_kart_cpp" + / "lib" + / "autonomous_kart_cpp" + / "camera_node" + ) + + def generate_launch_description(): pkg_share = get_package_share_directory("autonomous_kart") + camera_binary = _get_camera_binary(pkg_share) motor_yaml = os.path.join(pkg_share, "params", "motor.yaml") steering_yaml = os.path.join(pkg_share, "params", "steering.yaml") @@ -40,7 +66,7 @@ def generate_launch_description(): parameters=[steering_yaml], ), Node( - package="autonomous_kart", + package="autonomous_kart_cpp", executable="camera_node", name="camera_node", parameters=[camera_yaml], diff --git a/src/autonomous_kart/autonomous_kart/nodes/camera/camera_node.py b/src/autonomous_kart/autonomous_kart/nodes/camera/camera_node.py deleted file mode 100644 index 677c2c7d..00000000 --- a/src/autonomous_kart/autonomous_kart/nodes/camera/camera_node.py +++ /dev/null @@ -1,150 +0,0 @@ -import threading -import time -import traceback - -import cv2 -import rclpy -from rclpy.duration import Duration -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node -from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy -from sensor_msgs.msg import Image -from cv_bridge import CvBridge - - -class CameraNode(Node): - def __init__(self): - super().__init__( - "camera_node", - allow_undeclared_parameters=True, - automatically_declare_parameters_from_overrides=True - ) - self.last_callback_time = time.time() - self.logger = self.get_logger() - - self.fps = self.get_parameter("fps").value - self.frame_counter = 0 - - if self.fps == 0: # div by 0 error later - self.fps = self.get_parameter("system_frequency").value - - self.sim_mode = self.get_parameter("simulation_mode").value - - self.bridge = CvBridge() - - qos = QoSProfile( - depth=1, - reliability=ReliabilityPolicy.BEST_EFFORT, - durability=DurabilityPolicy.VOLATILE, - lifespan=Duration(seconds=0, nanoseconds=int(1e9 / self.fps)), - ) - self.image_pub = self.create_publisher(Image, "camera/image_raw", qos) - - if self.sim_mode: - video_path = "/ws/data/EVC_test_footage/video.mp4" - self.cap = cv2.VideoCapture(video_path) - if not self.cap.isOpened(): - self.logger.info(f"Failed to open video: {video_path}") - self.video_fps = self.cap.get(cv2.CAP_PROP_FPS) - - self.latest_frame = None - self.running = True - self.frame_lock = threading.Lock() - - self.reader_thread = threading.Thread(target=self.read_frames, daemon=True) - self.reader_thread.start() - - else: - self.cap = cv2.VideoCapture(0) # Real camera - self.video_fps = self.fps - - self.logger.info(f"Video FPS: {self.video_fps}, Given FPS: {self.fps}") - self.timer = self.create_timer(1.0 / self.fps, self.timer_callback) - - self.logger.info( - f"Camera Node started - Mode: {'SIM' if self.sim_mode else 'REAL'}" - ) - - def timer_callback(self): - """ - Publishes the next frame - """ - if self.sim_mode: - with self.frame_lock: - if self.latest_frame is not None: - self.image_pub.publish(self.latest_frame) - self.frame_counter += 1 - else: - self.logger.warning("Too slow frames - skipping") - if self.frame_counter % self.fps == 0: - now = time.time() - try: - actual_rate = self.fps / (now - self.last_callback_time) - except ZeroDivisionError: - actual_rate = 0 - - self.logger.info( - f"Published {self.frame_counter} frames, actual rate: {actual_rate:.1f} fps" - ) - self.last_callback_time = now - else: - # TODO: Implement real mode - pass - - def read_frames(self): - """ - Background thread to read frames for efficiency - """ - frame_time = 1.0 / self.video_fps - - while self.running: - start = time.time() - ret, frame = self.cap.read() - - # Loop video - if not ret: - self.cap.set(cv2.CAP_PROP_POS_FRAMES, 0) - ret, frame = self.cap.read() - else: - height, width = frame.shape[:2] - # 360x202 BGR image optimized for jetson communication - target_width = ( - 360 - ) - target_height = int(height * (target_width / width)) - resized = cv2.resize(frame, (target_width, target_height)) - msg = self.bridge.cv2_to_imgmsg(resized, "bgr8") - msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = "camera" - with self.frame_lock: - self.latest_frame = msg - - elapsed = time.time() - start - sleep_time = frame_time - elapsed - if sleep_time > 0: - time.sleep(sleep_time) - - -def main(args=None): - rclpy.init(args=args) - - node = CameraNode() - executor = MultiThreadedExecutor(num_threads=2) - executor.add_node(node) - - try: - executor.spin() - except KeyboardInterrupt: - pass - except Exception: - node.get_logger().error(traceback.format_exc()) - finally: - node.running = False - executor.shutdown(timeout_sec=1.0) - node.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/autonomous_kart/autonomous_kart/params/camera.yaml b/src/autonomous_kart/autonomous_kart/params/camera.yaml index 1abc96ad..40600300 100644 --- a/src/autonomous_kart/autonomous_kart/params/camera.yaml +++ b/src/autonomous_kart/autonomous_kart/params/camera.yaml @@ -1,4 +1,4 @@ # Camera config settings camera_node: ros__parameters: - fps: 60.0 # float \ No newline at end of file + fps: 0 # 0 == use system frequency \ No newline at end of file diff --git a/src/autonomous_kart/autonomous_kart/params/pathfinder.yaml b/src/autonomous_kart/autonomous_kart/params/pathfinder.yaml index b41ea455..db43a522 100644 --- a/src/autonomous_kart/autonomous_kart/params/pathfinder.yaml +++ b/src/autonomous_kart/autonomous_kart/params/pathfinder.yaml @@ -7,21 +7,21 @@ pathfinder_node: max_steering: 10 steering_accel: 0.05 line_path: /ws/data/racing_line/line.csv - + # Auto mode (Nav2 params) wheelbase_m: 1.05 # kart wheelbase in meters (rear axle -> front axle) v_max_mps: 15.0 # kart max speed in m/s (used for %<->m/s conversion) steer_max_deg: 25.0 # physical max front-wheel steering angle (degrees) - + use_velocity_scaled_lookahead: true # Nav2: use_velocity_scaled_lookahead_dist lookahead_time_s: 0.6 # Nav2: lookahead_time (s) min_lookahead_m: 3.0 # Nav2: min_lookahead_dist (m) max_lookahead_m: 8.0 # Nav2: max_lookahead_dist (m) - + use_curvature_regulation: true # Nav2: use_regulated_linear_velocity_scaling min_radius_m: 12.0 # Nav2: regulated_linear_scaling_min_radius (m) min_reg_speed_pct: 0.20 # like regulated_linear_scaling_min_speed / v_max - + approach_dist_m: 1.0 # Nav2: approach_velocity_scaling_dist (m) min_approach_speed_pct: 0.05 # like min_approach_linear_velocity / v_max @@ -29,4 +29,3 @@ pathfinder_node: initial_sync_done: false # First autonomous tick after pose_ready does an O(n) full-line nearest max_resync_dist: 80 # Max allowed distance to race line (m) before falling back to a full O(n) search max_closed_dist: 2 # Max distance between start and end for track to be considered closed (m) - \ No newline at end of file diff --git a/src/autonomous_kart/package.xml b/src/autonomous_kart/package.xml index 0005e101..da39534d 100644 --- a/src/autonomous_kart/package.xml +++ b/src/autonomous_kart/package.xml @@ -11,6 +11,7 @@ rclpy geometry_msgs + autonomous_kart_cpp ament_copyright ament_flake8 diff --git a/src/autonomous_kart/setup.py b/src/autonomous_kart/setup.py index 3d16c1d8..1d864076 100644 --- a/src/autonomous_kart/setup.py +++ b/src/autonomous_kart/setup.py @@ -1,26 +1,26 @@ from glob import glob from setuptools import setup -package_name = 'autonomous_kart' +package_name = "autonomous_kart" setup( name=package_name, version="0.0.1", packages=[ - 'autonomous_kart', - 'autonomous_kart.nodes', - 'autonomous_kart.nodes.motor', - 'autonomous_kart.nodes.steering', - 'autonomous_kart.nodes.camera', - 'autonomous_kart.nodes.gps', - 'autonomous_kart.nodes.pathfinder', - 'autonomous_kart.nodes.opencv_pathfinder', - 'autonomous_kart.nodes.e_comms', - 'autonomous_kart.nodes.3dgs_localization', - 'autonomous_kart.nodes.metrics', - 'autonomous_kart.nodes.master', - 'autonomous_kart.nodes.localization', - 'autonomous_kart.nodes.pathfinder.strategies', + "autonomous_kart", + "autonomous_kart.nodes", + "autonomous_kart.nodes.motor", + "autonomous_kart.nodes.steering", + "autonomous_kart.nodes.camera", + "autonomous_kart.nodes.gps", + "autonomous_kart.nodes.pathfinder", + "autonomous_kart.nodes.opencv_pathfinder", + "autonomous_kart.nodes.e_comms", + "autonomous_kart.nodes.3dgs_localization", + "autonomous_kart.nodes.metrics", + "autonomous_kart.nodes.master", + "autonomous_kart.nodes.localization", + "autonomous_kart.nodes.pathfinder.strategies", ], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), @@ -30,25 +30,24 @@ ], install_requires=["setuptools"], zip_safe=True, - maintainer='Purdue EVC', - maintainer_email='shay.manor@gmail.com', - license='Apache-2.0', + maintainer="Purdue EVC", + maintainer_email="shay.manor@gmail.com", + license="Apache-2.0", description="Package containing all nodes for driving in different states.", tests_require=["pytest"], entry_points={ - 'console_scripts': [ - 'motor_node = autonomous_kart.nodes.motor.motor_node:main', - 'pathfinder_node = autonomous_kart.nodes.pathfinder.pathfinder_node:main', - 'manual_pathfinder_api = autonomous_kart.nodes.manual.pathfinder_api:main', - 'gps_node = autonomous_kart.nodes.gps.gps_node:main', - '3dgs_localization_node = autonomous_kart.nodes.3dgs_localization.3dgs_localization_node:main', - 'opencv_pathfinder_node = autonomous_kart.nodes.opencv_pathfinder.opencv_pathfinder_node:main', - 'e_comms_node = autonomous_kart.nodes.e_comms.e_comms_node:main', - 'steering_node = autonomous_kart.nodes.steering.steering_node:main', - 'camera_node = autonomous_kart.nodes.camera.camera_node:main', - 'metrics_node = autonomous_kart.nodes.metrics.metrics_node:main', - 'master_api = autonomous_kart.nodes.master.master_api:main', - 'localization_node = autonomous_kart.nodes.localization.localization_node:main', + "console_scripts": [ + "motor_node = autonomous_kart.nodes.motor.motor_node:main", + "pathfinder_node = autonomous_kart.nodes.pathfinder.pathfinder_node:main", + "manual_pathfinder_api = autonomous_kart.nodes.manual.pathfinder_api:main", + "gps_node = autonomous_kart.nodes.gps.gps_node:main", + "3dgs_localization_node = autonomous_kart.nodes.3dgs_localization.3dgs_localization_node:main", + "opencv_pathfinder_node = autonomous_kart.nodes.opencv_pathfinder.opencv_pathfinder_node:main", + "e_comms_node = autonomous_kart.nodes.e_comms.e_comms_node:main", + "steering_node = autonomous_kart.nodes.steering.steering_node:main", + "metrics_node = autonomous_kart.nodes.metrics.metrics_node:main", + "master_api = autonomous_kart.nodes.master.master_api:main", + "localization_node = autonomous_kart.nodes.localization.localization_node:main", ], }, -) \ No newline at end of file +) diff --git a/src/autonomous_kart_cpp/CMakeLists.txt b/src/autonomous_kart_cpp/CMakeLists.txt new file mode 100644 index 00000000..0bf6dec6 --- /dev/null +++ b/src/autonomous_kart_cpp/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.8) +project(autonomous_kart_cpp) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(OpenCV REQUIRED) + +add_executable(camera_node src/camera_node.cpp) +ament_target_dependencies(camera_node rclcpp sensor_msgs cv_bridge OpenCV) + +install(TARGETS camera_node + DESTINATION lib/${PROJECT_NAME}) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/autonomous_kart_cpp/package.xml b/src/autonomous_kart_cpp/package.xml new file mode 100644 index 00000000..5a90f397 --- /dev/null +++ b/src/autonomous_kart_cpp/package.xml @@ -0,0 +1,23 @@ + + + + autonomous_kart_cpp + 0.0.0 + Package containing all high performance nodes for driving in different states. + root + TODO: License declaration + + ament_cmake + + rclcpp + sensor_msgs + cv_bridge + OpenCV + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/autonomous_kart_cpp/src/camera_node.cpp b/src/autonomous_kart_cpp/src/camera_node.cpp new file mode 100644 index 00000000..26623d6d --- /dev/null +++ b/src/autonomous_kart_cpp/src/camera_node.cpp @@ -0,0 +1,183 @@ +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "cv_bridge/cv_bridge.h" +#include "opencv2/opencv.hpp" +#include +#include + +class CameraNode : public rclcpp::Node +{ +public: + CameraNode() + : Node("camera_node"), frame_counter_(0), running_(true) + { + // --- Parameters --- + this->declare_parameter("simulation_mode", false); + this->declare_parameter("fps", 0); + this->declare_parameter("system_frequency", 10); + + sim_mode_ = this->get_parameter("simulation_mode").as_bool(); + fps_ = this->get_parameter("fps").as_int(); + if (fps_ <= 0) + fps_ = this->get_parameter("system_frequency").as_int(); + // --- QoS Profile --- + rclcpp::QoS qos(1); + qos.best_effort(); + qos.durability_volatile(); + qos.lifespan(rclcpp::Duration::from_seconds(1.0 / fps_)); + + image_pub_ = this->create_publisher("camera/image_raw", qos); + + // --- Initialize OpenCV --- + if (sim_mode_) + { + std::string video_path = "/ws/data/EVC_test_footage/vid.mp4"; // using low quality video for performance + cap_.open(video_path); + if (!cap_.isOpened()) + { + RCLCPP_ERROR(this->get_logger(), "Failed to open video: %s", video_path.c_str()); + } + video_fps_ = cap_.get(cv::CAP_PROP_FPS); + if (video_fps_ <= 0.0) + { + video_fps_ = 60.0; + } + reader_thread_ = std::thread(&CameraNode::read_frames, this); + } + else + { + cap_.open(0); + } + + RCLCPP_INFO(this->get_logger(), "Video FPS: %.2f, Given FPS: %.2f", video_fps_, fps_); + + timer_ = this->create_wall_timer( + std::chrono::duration(1.0 / fps_), + std::bind(&CameraNode::timer_callback, this)); + + RCLCPP_INFO(this->get_logger(), "CPP Camera Node started - Mode: %s", + sim_mode_ ? "SIM" : "REAL"); + } + + ~CameraNode() override + { + running_ = false; + if (reader_thread_.joinable()) + reader_thread_.join(); + cap_.release(); + } + +private: + void timer_callback() + { + std::lock_guard lock(frame_mutex_); + if (latest_frame_) + { + auto start = std::chrono::steady_clock::now(); + image_pub_->publish(*latest_frame_); + frame_counter_++; + auto end = std::chrono::steady_clock::now(); + auto elapsed = end - start; + RCLCPP_DEBUG(this->get_logger(), "Published frame in %.2f ms", + std::chrono::duration(elapsed).count()); + } + else + { + RCLCPP_WARN(this->get_logger(), "No frame yet, skipping"); + } + } + + // array of frame times + std::vector frame_times_; + void read_frames() + { + cv_bridge::CvImage bridge; + const double frame_time = 1.0 / video_fps_; + + while (rclcpp::ok() && running_) + { + auto start = std::chrono::steady_clock::now(); + cv::Mat frame; + if (!cap_.read(frame)) + { + cap_.set(cv::CAP_PROP_POS_FRAMES, 0); + continue; + } + + auto loadImageTime = std::chrono::steady_clock::now(); + + // Resize + int width = 360; + int height = static_cast(frame.rows * (360.0 / frame.cols)); + cv::resize(frame, frame, cv::Size(width, height)); + auto resizeTime = std::chrono::steady_clock::now(); + + // Convert to ROS image + bridge.encoding = "bgr8"; + bridge.image = frame; + bridge.header.stamp = this->now(); + bridge.header.frame_id = "camera"; + + { + std::lock_guard lock(frame_mutex_); + latest_frame_ = bridge.toImageMsg(); + } + + auto now = std::chrono::steady_clock::now(); + auto elapsed = now - start; // in nanoseconds + auto sleep_time = std::chrono::duration(frame_time) - elapsed; + + frame_times_.push_back(std::chrono::duration(elapsed).count()); + + if (sleep_time.count() > 0 && frame_times_.size() % 1000 != 0) + { + std::this_thread::sleep_for(sleep_time); + } + else + { + // Frame time logging every 1000 frames or if we're slower than real-time + RCLCPP_WARN(this->get_logger(), "Processing is slower than frame time at %.2f / %.2f ms", elapsed.count() / 1e6, frame_time * 1000); + float avg_frame_time = 0.0; + for (const auto &t : frame_times_) + { + avg_frame_time += t; + } + avg_frame_time /= frame_times_.size(); + RCLCPP_WARN(this->get_logger(), "Average frame time over %zu frames: %.2f ms", frame_times_.size(), avg_frame_time); + // Print time to proccess each stage + RCLCPP_INFO(this->get_logger(), "CPP Camera Node full frame time: %.2f ms, Load Image Time: %.2f ms, Resize Time: %.2f ms", + process_time_ms(start, now), process_time_ms(start, loadImageTime), process_time_ms(loadImageTime, resizeTime)); + frame_times_.clear(); + } + } + } + + float process_time_ms(const std::chrono::steady_clock::time_point &start, const std::chrono::steady_clock::time_point &end) + { + auto elapsed = end - start; + return std::chrono::duration(elapsed).count(); + } + // --- Members --- + rclcpp::Publisher::SharedPtr image_pub_; + rclcpp::TimerBase::SharedPtr timer_; + cv::VideoCapture cap_; + double fps_; + double video_fps_; + bool sim_mode_; + bool running_; + size_t frame_counter_; + std::shared_ptr latest_frame_; + std::mutex frame_mutex_; + std::thread reader_thread_; +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +}