diff --git a/.gitignore b/.gitignore index 100dc30..266180e 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,3 @@ *bag +.vscode/ +data/ diff --git a/CMakeLists.txt b/CMakeLists.txt index d7687fe..f8f425f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.8) include(CMakePrintHelpers) -set(CMAKE_CUDA_ARCHITECTURES 60 61 62 70 72 75 86) +set(CMAKE_CUDA_ARCHITECTURES 86 87 89) set(CMAKE_CUDA_COMPILER /usr/local/cuda/bin/nvcc) project(visionsystemx LANGUAGES C CXX CUDA) @@ -29,8 +29,11 @@ find_package(pcl_conversions REQUIRED) find_package(message_filters REQUIRED) find_package(Eigen3 REQUIRED) -set(TensorRT_INCLUDE_DIRS /usr/include/aarch64-linux-gnu) -set(TensorRT_LIBRARIES /usr/lib/aarch64-linux-gnu) +if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64") + set(ARCH_INCLUDE_PATH /usr/include/aarch64-linux-gnu) +else() + set(ARCH_INCLUDE_PATH /usr/include/x86_64-linux-gnu) +endif() add_executable(beeblebrox src/beeblebrox.cpp) @@ -44,7 +47,7 @@ add_executable(yolo src/detectors/yolo_tensorrt/yolo_tensorrt_node.cpp) ament_target_dependencies(yolo rclcpp std_msgs OpenCV cv_bridge image_transport CUDA ZED usv_interfaces ) -target_include_directories(yolo PRIVATE ${CUDA_INCLUDE_DIRS}) +target_include_directories(yolo PRIVATE ${ARCH_INCLUDE_PATH} ${CUDA_INCLUDE_DIRS}) target_link_libraries(yolo ${CUDA_LIBRARIES} ${TensorRT_LIBRARIES} nvinfer nvinfer_plugin) add_executable(lidar src/lidar/translation.cpp) diff --git a/launch/vision.launch.py b/launch/vision.launch.py index 4b20487..90a9132 100644 --- a/launch/vision.launch.py +++ b/launch/vision.launch.py @@ -5,6 +5,7 @@ from launch.substitutions import PathJoinSubstitution, LaunchConfiguration from launch_ros.substitutions import FindPackageShare from launch_ros.actions import Node +import os def generate_launch_description(): camera_model_arg = DeclareLaunchArgument( @@ -89,7 +90,21 @@ def generate_launch_description(): package='visionsystemx', executable='lidar_camera.py' ) - + + # Republish raw image as compressed for the web dashboard (rosbridge). + # Doesn't affect anything else + image_republish = Node( + package='image_transport', + executable='republish', + name='image_republish', + arguments=['raw', 'compressed'], + remappings=[ + ('in', '/bebblebrox/video/image'), + ('out/compressed', '/bebblebrox/video/compressed'), + ], + output='screen', + ) + return LaunchDescription([ camera_model_arg, video_feed, diff --git a/src/detectors/yolo_tensorrt/yolo_tensorrt.hpp b/src/detectors/yolo_tensorrt/yolo_tensorrt.hpp index b39eafc..67e25f1 100644 --- a/src/detectors/yolo_tensorrt/yolo_tensorrt.hpp +++ b/src/detectors/yolo_tensorrt/yolo_tensorrt.hpp @@ -440,6 +440,10 @@ void YOLOv8::draw_objects( std::string label_text = cv::format("%s %d%%", CLASS_NAMES[obj.label].c_str(), (int)(obj.prob*100.0)); + std::string label_text = cv::format("%s %d%%", + CLASS_NAMES[obj.label].c_str(), (int)(obj.prob*100.0)); + + // Calculate text size and position int baseline = 0; cv::Size label_size = cv::getTextSize(label_text, cv::FONT_HERSHEY_SIMPLEX, 0.4, 1, &baseline); diff --git a/src/detectors/yolo_tensorrt/yolo_tensorrt_node.cpp b/src/detectors/yolo_tensorrt/yolo_tensorrt_node.cpp index 5aaeea0..89d5fba 100644 --- a/src/detectors/yolo_tensorrt/yolo_tensorrt_node.cpp +++ b/src/detectors/yolo_tensorrt/yolo_tensorrt_node.cpp @@ -16,12 +16,12 @@ const std::vector NAMES { "black_buoy", "blue_buoy", "course_marker", - "port_marker", - "starboard_marker", - "yellow_marker", - "blue_circle", - "blue_cross", - "blue_square" + "green_buoy", + "port_marker", + "red_buoy", + "starboard_marker", + "yellow_marker", + "blue_square", "blue_triangle", "green_circle", "green_cross", @@ -31,8 +31,8 @@ const std::vector NAMES { "red_cross", "red_square", "red_triangle", - "green_buoy", - "red_buoy" + "blue_circle", + "blue_cross" }; const std::vector> COLORS { diff --git a/src/zed.hpp b/src/zed.hpp index bf9ca2c..b67f84a 100644 --- a/src/zed.hpp +++ b/src/zed.hpp @@ -26,15 +26,8 @@ ZED_usv::ZED_usv(rclcpp::Logger logger_param, const std::string& camera_model) : bool is_zed1 = (camera_model == "zed" || camera_model == "zed1"); RCLCPP_INFO(this->logger, "Initializing camera model: %s", camera_model.c_str()); - // OPTIMIZED SETTINGS FOR JETSON ORIN NX AND NANO - USB STABILITY FIX - - init_params.sdk_verbose = true; // ORIGINAL - // init_params.sdk_verbose = false; // OPTIMIZED - - - init_params.depth_mode = sl::DEPTH_MODE::ULTRA; // ORIGINAL - // init_params.depth_mode = sl::DEPTH_MODE::NEURAL_LIGHT; // OPTIMIZED - + init_params.sdk_verbose = true; + init_params.depth_mode = sl::DEPTH_MODE::NEURAL; // Ultra is deprecated in newest sdk init_params.coordinate_system = sl::COORDINATE_SYSTEM::LEFT_HANDED_Z_UP; init_params.coordinate_units = sl::UNIT::METER; init_params.depth_maximum_distance = 20;