diff --git a/.gitignore b/.gitignore index f340edc..baa81f5 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,10 @@ # Visual Studio Code files -.vscode \ No newline at end of file +.vscode + +# Fuzzing +tags +build +.cache +install +log diff --git a/CMakeLists.txt b/CMakeLists.txt index 615327b..2347d82 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,8 +47,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -) + add_compile_options(-Wall -Wextra -Wpedantic) endif() set(opencv_version 4) @@ -86,34 +85,68 @@ if(NOT OpenCV_FOUND) find_package(OpenCV 3 REQUIRED COMPONENTS imgproc highgui) endif() - if (BUILD_GSTREAMER) -pkg_check_modules(GST REQUIRED IMPORTED_TARGET gstreamer-1.0) -include_directories(SYSTEM ${GST_INCLUDE_DIRS}) -link_directories(${GST_LIBRARY_DIRS}) + pkg_check_modules(GST REQUIRED IMPORTED_TARGET gstreamer-1.0) + include_directories(SYSTEM ${GST_INCLUDE_DIRS}) + link_directories(${GST_LIBRARY_DIRS}) endif() include_directories(include) set(camera_dependencies -rclcpp -sensor_msgs -stereo_msgs -geometry_msgs -std_msgs -vision_msgs -rclcpp_components -camera_info_manager -cv_bridge -image_transport + rclcpp + sensor_msgs + stereo_msgs + geometry_msgs + std_msgs + vision_msgs + rclcpp_components + camera_info_manager + cv_bridge + image_transport ) FILE(GLOB LIB_SRC -"src/ImageConverter.cpp" -"src/ImgDetectionConverter.cpp" -"src/ImuConverter.cpp" + "src/ImageConverter.cpp" + "src/ImgDetectionConverter.cpp" + "src/ImuConverter.cpp" + "src/DisparityConverter.cpp" ) +option(FUZZING "Enable fuzzing mode" OFF) + + +#find_library(ICU_LIBRARIES NAMES icuuc icudata icui18n PATHS /usr/lib/x86_64-linux-gnu) + +if(FUZZING) + # Enable fuzzing and sanitizers + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=fuzzer,address,undefined -ggdb -O2") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=fuzzer,address,undefined -ggdb -O2") + set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -fsanitize=fuzzer,address,undefined -ggdb -O2") + + # Add the depthai_bridge library + add_library(depthai_bridge SHARED ${LIB_SRC}) + ament_target_dependencies(depthai_bridge PUBLIC ${camera_dependencies}) + target_link_libraries(depthai_bridge PUBLIC depthai::core opencv_imgproc opencv_highgui) + + # NOTE: This is copied from below as it enables correct compilation for + # the `ImgDetectionConverter.cpp` + target_compile_definitions(depthai_bridge PRIVATE IS_HUMBLE) + + add_executable(img_conv_fuzz fuzz/img_conv_fuzz.cc) + target_link_libraries(img_conv_fuzz PRIVATE depthai_bridge) + add_executable(imu_conv_fuzz fuzz/imu_conv_fuzz.cc) + target_link_libraries(imu_conv_fuzz PRIVATE depthai_bridge) + add_executable(img_det_conv_fuzz fuzz/img_det_conv_fuzz.cc) + target_link_libraries(img_det_conv_fuzz PRIVATE depthai_bridge) + add_executable(disp_conv_fuzz fuzz/disp_fuzz.cc) + target_link_libraries(disp_conv_fuzz PRIVATE depthai_bridge) + + # Stop after building the fuzz target + return() +endif() + +# Regular build process continues here add_library(depthai_bridge SHARED ${LIB_SRC}) ament_target_dependencies(depthai_bridge PUBLIC ${camera_dependencies}) target_link_libraries(depthai_bridge PUBLIC depthai-core opencv_imgproc opencv_highgui) @@ -134,7 +167,6 @@ add_library(depthai_camera SHARED src/depthai_camera.cpp) ament_target_dependencies(depthai_camera PUBLIC ${camera_dependencies}) target_link_libraries(depthai_camera PUBLIC depthai-core opencv_imgproc opencv_highgui depthai_bridge) - # DepthAI Camera as separate node add_executable(camera_node src/camera_node.cpp) ament_target_dependencies(camera_node PUBLIC rclcpp rclcpp_components) @@ -145,32 +177,31 @@ add_dependencies(camera_node depthai_camera) rclcpp_components_register_nodes(depthai_camera PLUGIN "${PROJECT_NAME}::DepthAICamera" EXECUTABLE depthai_camera) if (BUILD_GSTREAMER) -# DepthAI GStreamer as Component library -add_library(gstreamer_interface SHARED src/gstreamer_interface.cpp) -ament_target_dependencies(gstreamer_interface PUBLIC rclcpp std_msgs sensor_msgs) -target_link_libraries(gstreamer_interface PUBLIC ${GST_LIBRARIES} gstapp-1.0) - -# DepthAI GStreamer as Component library -add_library(depthai_gstreamer SHARED src/depthai_gstreamer.cpp) -ament_target_dependencies(depthai_gstreamer PUBLIC rclcpp std_msgs sensor_msgs rclcpp_components) -target_link_libraries(depthai_gstreamer PUBLIC ${GST_LIBRARIES} gstapp-1.0 gstreamer_interface) - -# DepthAI GStreamer as separate node -add_executable(gstreamer_node src/gstreamer_node.cpp) -ament_target_dependencies(gstreamer_node PUBLIC rclcpp rclcpp_components) -target_link_libraries(gstreamer_node PUBLIC depthai_gstreamer) -# Combined DepthAI node -add_executable(depthai_ctrl src/depthai_ctrl.cpp) -ament_target_dependencies(depthai_ctrl PUBLIC rclcpp rclcpp_components) -target_link_libraries(depthai_ctrl PUBLIC depthai_camera depthai_gstreamer) - -add_dependencies(depthai_ctrl depthai_gstreamer depthai_camera) -add_dependencies(gstreamer_node depthai_gstreamer) -rclcpp_components_register_nodes(depthai_gstreamer PLUGIN "${PROJECT_NAME}::DepthAIGStreamer" EXECUTABLE depthai_gstreamer) + # DepthAI GStreamer as Component library + add_library(gstreamer_interface SHARED src/gstreamer_interface.cpp) + ament_target_dependencies(gstreamer_interface PUBLIC rclcpp std_msgs sensor_msgs) + target_link_libraries(gstreamer_interface PUBLIC ${GST_LIBRARIES} gstapp-1.0) + + # DepthAI GStreamer as Component library + add_library(depthai_gstreamer SHARED src/depthai_gstreamer.cpp) + ament_target_dependencies(depthai_gstreamer PUBLIC rclcpp std_msgs sensor_msgs rclcpp_components) + target_link_libraries(depthai_gstreamer PUBLIC ${GST_LIBRARIES} gstapp-1.0 gstreamer_interface) + + # DepthAI GStreamer as separate node + add_executable(gstreamer_node src/gstreamer_node.cpp) + ament_target_dependencies(gstreamer_node PUBLIC rclcpp rclcpp_components) + target_link_libraries(gstreamer_node PUBLIC depthai_gstreamer) + + # Combined DepthAI node + add_executable(depthai_ctrl src/depthai_ctrl.cpp) + ament_target_dependencies(depthai_ctrl PUBLIC rclcpp rclcpp_components) + target_link_libraries(depthai_ctrl PUBLIC depthai_camera depthai_gstreamer) + + add_dependencies(depthai_ctrl depthai_gstreamer depthai_camera) + add_dependencies(gstreamer_node depthai_gstreamer) + rclcpp_components_register_nodes(depthai_gstreamer PLUGIN "${PROJECT_NAME}::DepthAIGStreamer" EXECUTABLE depthai_gstreamer) endif() - - # Disable tests as its obsolete # if (BUILD_TESTING) # enable_testing() @@ -214,9 +245,7 @@ install(TARGETS camera_node DESTINATION lib/${PROJECT_NAME}) - install(DIRECTORY launch params urdf - DESTINATION share/${PROJECT_NAME} - ) + DESTINATION share/${PROJECT_NAME}) -ament_package() \ No newline at end of file +ament_package() diff --git a/fuzz/disp_fuzz.cc b/fuzz/disp_fuzz.cc new file mode 100644 index 0000000..6d69eec --- /dev/null +++ b/fuzz/disp_fuzz.cc @@ -0,0 +1,146 @@ +#include +#include + +#include "depthai-shared/common/CameraInfo.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +dai::RawImgFrame::Type PickFrameType(FuzzedDataProvider &fdp) +{ + return fdp.PickValueInArray( + {dai::RawImgFrame::Type::YUV422i, dai::RawImgFrame::Type::YUV444p, + dai::RawImgFrame::Type::YUV420p, dai::RawImgFrame::Type::YUV422p, + dai::RawImgFrame::Type::YUV400p, dai::RawImgFrame::Type::RGBA8888, + dai::RawImgFrame::Type::RGB161616, dai::RawImgFrame::Type::RGB888p, + dai::RawImgFrame::Type::BGR888p, dai::RawImgFrame::Type::RGB888i, + dai::RawImgFrame::Type::BGR888i, dai::RawImgFrame::Type::RGBF16F16F16p, + dai::RawImgFrame::Type::BGRF16F16F16p, dai::RawImgFrame::Type::RGBF16F16F16i, + dai::RawImgFrame::Type::BGRF16F16F16i, dai::RawImgFrame::Type::GRAY8, + dai::RawImgFrame::Type::GRAYF16, dai::RawImgFrame::Type::LUT2, + dai::RawImgFrame::Type::LUT4, dai::RawImgFrame::Type::LUT16, + dai::RawImgFrame::Type::RAW16, dai::RawImgFrame::Type::RAW14, + dai::RawImgFrame::Type::RAW12, dai::RawImgFrame::Type::RAW10, + dai::RawImgFrame::Type::RAW8, dai::RawImgFrame::Type::PACK10, + dai::RawImgFrame::Type::PACK12, dai::RawImgFrame::Type::YUV444i, + dai::RawImgFrame::Type::NV12, dai::RawImgFrame::Type::NV21, + dai::RawImgFrame::Type::BITSTREAM, dai::RawImgFrame::Type::HDR, + dai::RawImgFrame::Type::NONE}); +} + +std::chrono::time_point GenerateFuzzTimestamp(FuzzedDataProvider &fdp) +{ + using namespace std::chrono; + auto now = steady_clock::now(); + auto now_ts = duration_cast(now.time_since_epoch()).count(); + + // Range: ±1 day in nanoseconds + constexpr uint64_t ONE_DAY_NS = 86400ULL * 1000000000ULL; + + uint64_t fuzz_ns = fdp.ConsumeIntegralInRange(now_ts - ONE_DAY_NS, // min: now - 1 day + now_ts + ONE_DAY_NS // max: now + 1 day + ); + + // Convert back to time_point by creating from epoch + return steady_clock::time_point(nanoseconds(fuzz_ns)); +} + +extern "C" int LLVMFuzzerTestOneInput(const uint8_t *data, size_t size) +{ + + if (size < 64) + { + return -1; + } + + FuzzedDataProvider fdp(data, size); + + std::string frameName = fdp.ConsumeRandomLengthString(); + float focalLength = fdp.ConsumeFloatingPoint(); + float baseline = fdp.ConsumeFloatingPoint(); + float minDepth = fdp.ConsumeFloatingPoint(); + float maxDepth = fdp.ConsumeFloatingPoint(); + + // printf("FOO\n"); + + try + { + dai::ros::DisparityConverter converter(frameName, focalLength, baseline, minDepth, maxDepth); + // dai::ros::DisparityConverter converter("asdf", 1.2); + // printf("BAR\n"); + + auto category = fdp.ConsumeIntegral(); + auto height = fdp.ConsumeIntegral(); + auto instanceNum = fdp.ConsumeIntegral(); + auto seqNum = fdp.ConsumeIntegral(); + auto width = fdp.ConsumeIntegral(); + auto type = PickFrameType(fdp); + auto ts = GenerateFuzzTimestamp(fdp); + + // Determine the number of elements for the vector + size_t num_elements = fdp.ConsumeIntegralInRange(1, 100); + + // Create a vector to hold the integers + std::vector int_list; + + // Populate the vector by consuming integers from FuzzedDataProvider + for (size_t i = 0; i < num_elements; i++) + { + int_list.push_back(fdp.ConsumeIntegral()); + } + + // Convert the std::vector to std::vector + std::vector byte_list; + for (const auto &val : int_list) + { + // Safely cast/convert each int value to uint8_t + byte_list.push_back(static_cast(val & 0xFF)); // Mask to 8 bits + } + + auto inData = std::make_shared(); + inData->setWidth(width); // Image width in pixels + inData->setHeight(height); // Image height in pixels + inData->setCategory(category); + inData->setType(type); // Format/type (e.g.,dai::RawImgFrame::Type::BGR888) + inData->setData(byte_list); // Raw image data buffer + // FIXME: + inData->setTimestamp(ts); // Timestamp + inData->setSequenceNum(seqNum); // Sequence number + inData->setInstanceNum(instanceNum); // Instance number + + std::deque outImageMsg; + converter.toRosMsg(inData, outImageMsg); + } + catch (const std::exception &e) + { + printf("[!] ERR"); + return -1; + } + + return 0; +} + +// 1. NULL PTR into memcpy as dst +// --> DisparityConverter:75 (crash-adc...2fc) + +// 2. Multiple OOM situations in toRosMsg where aaray allocation takes place. +// --> e.g. ./oom-ba3c2d81f661ecb7fffa1aef25893db89f10b297 + +// 3. /home/user/git/work/depthai_ctrl/src/DisparityConverter.cpp:99:65: runtime +// error: -8 is outside the range of representable values of type 'unsigned +// long' SUMMARY: UndefinedBehaviorSanitizer: undefined-behavior +// /home/user/git/work/depthai_ctrl/src/DisparityConverter.cpp:99:65 +// std::transform(raw16Data.begin(), raw16Data.end(), +// std::back_inserter(convertedData), +// [](int16_t disp) -> std::size_t { return +// static_cast(disp) / 32.0; }); + +// 4. Dvi by 0 +// outImageMsg.step = size / inData->getHeight(); +// crash-crash-4b3043662cdde20ecb4bfc70fa0ca03001438378 diff --git a/fuzz/img_conv_fuzz.cc b/fuzz/img_conv_fuzz.cc new file mode 100644 index 0000000..eb537d1 --- /dev/null +++ b/fuzz/img_conv_fuzz.cc @@ -0,0 +1,286 @@ +#include +#include +#include + +#include "depthai-shared/common/CameraInfo.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// clang-format off +static const char* knownErrors[] = { + "Division by 0", + "imdecode", + "depthOut empty", + nullptr +}; +// clang-format on + +const std::vector ENCODINGS = { + "rgb8", "rgba8", "rgb16", "rgba16", "bgr8", "bgra8", "bgr16", "bgra16", + "mono8", "mono16", "8UC1", "8UC3", "16UC1", "16UC3", "32FC1", "32FC3", + "64FC1", "64FC3", "bayer_rggb8", "bayer_bggr8", "bayer_gbrg8", "bayer_grbg8", "yuv422"}; + +sensor_msgs::msg::Image generateFuzzedImage(FuzzedDataProvider &fdp) +{ + sensor_msgs::msg::Image img; + + // Generate header + img.header.stamp = rclcpp::Time(fdp.ConsumeIntegral()); // seconds + img.header.frame_id = fdp.ConsumeRandomLengthString(16); // frame_id + + // Generate dimensions (with reasonable bounds to avoid excessive memory + // usage) + img.height = fdp.ConsumeIntegralInRange(1, 2048); + img.width = fdp.ConsumeIntegralInRange(1, 2048); + + // Pick a random encoding from common types + img.encoding = ENCODINGS[fdp.ConsumeIntegralInRange(0, ENCODINGS.size() - 1)]; + + // Generate endianness + img.is_bigendian = fdp.ConsumeBool(); + + // Calculate step based on encoding and width + // This is a simplified calculation - you might want to make it more precise + // based on your specific encodings + size_t bytes_per_pixel; + if (img.encoding.find("8") != std::string::npos) + { + bytes_per_pixel = 1; + } + else if (img.encoding.find("16") != std::string::npos) + { + bytes_per_pixel = 2; + } + else if (img.encoding.find("32") != std::string::npos) + { + bytes_per_pixel = 4; + } + else if (img.encoding.find("64") != std::string::npos) + { + bytes_per_pixel = 8; + } + else + { + bytes_per_pixel = 1; // default + } + + size_t channels; + if (img.encoding.find("C3") != std::string::npos || img.encoding.find("rgb") != std::string::npos || + img.encoding.find("bgr") != std::string::npos) + { + channels = 3; + } + else if (img.encoding.find("rgba") != std::string::npos || img.encoding.find("bgra") != std::string::npos) + { + channels = 4; + } + else + { + channels = 1; // default to single channel + } + + img.step = img.width * bytes_per_pixel * channels; + + // Generate image data + size_t data_size = img.step * img.height; + img.data.resize(data_size); + + // Fill with random data + std::vector random_data = fdp.ConsumeBytes(data_size); + if (!random_data.empty()) + { + std::copy(random_data.begin(), random_data.end(), img.data.begin()); + } + + return img; +} + +extern "C" int LLVMFuzzerTestOneInput(const uint8_t *Data, size_t Size) +{ + FuzzedDataProvider fdp(Data, Size); + + auto inData = std::make_shared(); + + unsigned int width = fdp.ConsumeIntegral(); + unsigned int height = fdp.ConsumeIntegral(); + unsigned int channelSize = fdp.ConsumeIntegral(); + size_t imageSize = width * height * channelSize; + std::vector frameData = fdp.ConsumeBytes(imageSize); + + // Set the generated data to the ImgFrame object + inData->setWidth(width); + inData->setHeight(height); + inData->setData(frameData); + + // NOTE: Check if inData is populated + // Otherwise we're triggering an assert: + // what(): OpenCV(4.6.0) ./modules/imgcodecs/src/loadsave.cpp:816: error: + // (-215:Assertion failed) !buf.empty() in function 'imdecode_' + // if (inData->getData().empty()) + //{ + // return 1; + //} + + // NOTE: Another sanity check for: + // what(): OpenCV(4.6.0) + // /usr/include/opencv4/opencv2/core/utility.hpp:627: error: + // (-215:Assertion failed) !empty() in function 'forEach_impl' + // if (width <= 0 || height <= 0) + //{ + // return 1; + //} + + std::deque outImageMsgs; + dai::RawImgFrame::Type type; + // NOTE: Some are throwing what(): Converted type not supported! + switch (fdp.ConsumeIntegralInRange(0, 2)) + { + case 0: + type = dai::RawImgFrame::Type::RAW8; + break; + case 1: + type = dai::RawImgFrame::Type::GRAY8; + break; + case 2: + type = dai::RawImgFrame::Type::BGR888i; + break; + case 3: + type = dai::RawImgFrame::Type::RGBA8888; + break; + case 4: + type = dai::RawImgFrame::Type::RGB888i; + break; + case 5: + type = dai::RawImgFrame::Type::RAW16; + break; + case 6: + type = dai::RawImgFrame::Type::YUV420p; + break; + default: + type = dai::RawImgFrame::Type::RAW8; + } + inData->setType(type); + + inData->setCategory(fdp.ConsumeIntegral()); + inData->setInstanceNum(fdp.ConsumeIntegral()); + inData->setSequenceNum(fdp.ConsumeIntegral()); + + sensor_msgs::msg::CameraInfo info; + std::string frameName = fdp.ConsumeRandomLengthString(); + bool interleaved = fdp.ConsumeBool(); + + dai::ros::ImageConverter imageConverter(frameName, interleaved); + + try + { + switch (fdp.ConsumeIntegralInRange(0, 4)) + { + case 0: + imageConverter.toRosMsgFromBitStream(inData, outImageMsgs, type, info); + break; + case 1: + imageConverter.toRosMsg(inData, outImageMsgs); + break; + case 2: + imageConverter.toRosMsgPtr(inData); + break; + case 3: { + auto fuzzed_image = generateFuzzedImage(fdp); + dai::ImgFrame outData; + imageConverter.toDaiMsg(fuzzed_image, outData); + break; + } + case 4: { + auto fuzzed_image2 = generateFuzzedImage(fdp); + imageConverter.rosMsgtoCvMat(fuzzed_image2); + break; + } + case 5: { + // ImageMsgs::CameraInfo + // ImageConverter::calibrationToCameraInfo(dai::CalibrationHandler + // calibHandler, + // dai::CameraBoardSocket + // cameraId, int + // width, int height, + // Point2f + // topLeftPixelId, + // Point2f + // bottomRightPixelId) + break; + } + } + } + // TODO: calibrationToCameraInfo + catch (const std::runtime_error &e) + { + // Handle runtime errors separately + const char *errorMsg = e.what(); + for (const char **known = knownErrors; *known != nullptr; ++known) + { + if (strstr(errorMsg, *known) != nullptr) + { + + inData.reset(); + outImageMsgs.clear(); // Clear before exit + + return 0; + } + } + inData.reset(); + outImageMsgs.clear(); // Clear before exit + + throw; + } + + // Check if the output image messages are valid + // for (const auto &msg : outImageMsgs) + //{ + // if (msg.width <= 0 || msg.height <= 0 || msg.encoding.empty()) + // { + // throw(std::runtime_error("Received invalid image back")); + // return -1; + // } + //} + inData.reset(); + outImageMsgs.clear(); // Clear before exit + + return 0; +} + +//+ 1. assertion trigger in opencv -> crash-d22580....ec5 (!empty()) +//+ 2. assertion trigger in opencv -> crash-c064...b50 (validateInputImageSize) + +// ? 3. OOB-R in ImageConverter::toDaiMsg:281 -> crash-ce84...5e15 + +// + 4. deadly signal what stoi: in ImageConverter:261 (stoi) -> +// crash-7f64...bf09 (invalid argument) +// -> in particular the call to stoi(encoding_info[1]) +// -> RCA: The while loop before just adds a single element in slot [0] +// -> encoding_info[1] is really an OOB access + +// + 5. Another SEGV (OOB) right befor the while-loop revEncodingIter->second in +// ImageConverter.cpp:260 +// -> crash-22f7....afa67 + +// + 6. Address Sanitizer FPE - Division by 0 +// -> outImageMsg.step = inData->getData().size() / inData->getHeight(); +// (crash-21d0....8f48) + +// + 7. Deadly signal UBSAN memcpy 2nd argument 0 (crash-8deac....f8202) + +// 8. SUMMARY: UndefinedBehaviorSanitizer: undefined-behavior +// /usr/bin/../lib/gcc/x86_64-linux-gnu/13/../../../../include/c++/13/bits/basic_string.h:551:48 +// terminate called after throwing an instance of 'std::length_error' +// what(): basic_string::_M_create +// toRosMsgPtr(std::shared_ptr) ImageConverter.cpp:316:16 +// ---> crash-b8b....9dec diff --git a/fuzz/img_det_conv_fuzz.cc b/fuzz/img_det_conv_fuzz.cc new file mode 100644 index 0000000..4e301fd --- /dev/null +++ b/fuzz/img_det_conv_fuzz.cc @@ -0,0 +1,87 @@ +#include +#include + +#include "depthai-shared/common/CameraInfo.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +// clang-format off +static const char* knownErrors[] = { + "Time difference would underflow", + "Time stamp calculation would overflow", + nullptr +}; +// clang-format on + +extern "C" int LLVMFuzzerTestOneInput(const uint8_t *Data, size_t Size) +{ + FuzzedDataProvider dataProvider(Data, Size); + + std::string frameName = dataProvider.ConsumeRandomLengthString(); + int width = dataProvider.ConsumeIntegral(); + int height = dataProvider.ConsumeIntegral(); + bool normalized = dataProvider.ConsumeBool(); + + dai::ros::ImgDetectionConverter detectionConverter(frameName, width, height, normalized); + + auto inNetData = std::make_shared(); + + // Determine the number of detections to generate + size_t numDetections = dataProvider.ConsumeIntegralInRange(0, 10); + + // Generate random detections and add them to inNetData->detections + for (size_t i = 0; i < numDetections; ++i) + { + dai::ImgDetection detection; + + detection.label = dataProvider.ConsumeIntegral(); + detection.confidence = dataProvider.ConsumeFloatingPoint(); + detection.xmin = dataProvider.ConsumeFloatingPoint(); + detection.ymin = dataProvider.ConsumeFloatingPoint(); + detection.xmax = dataProvider.ConsumeFloatingPoint(); + detection.ymax = dataProvider.ConsumeFloatingPoint(); + + inNetData->detections.push_back(detection); + } + + auto timestamp = std::chrono::steady_clock::time_point( + std::chrono::steady_clock::duration(dataProvider.ConsumeIntegral())); + inNetData->setTimestamp(timestamp); + + auto timestampDevice = std::chrono::steady_clock::time_point( + std::chrono::steady_clock::duration(dataProvider.ConsumeIntegral())); + inNetData->setTimestampDevice(timestampDevice); + + inNetData->setSequenceNum(dataProvider.ConsumeIntegral()); + + std::deque opDetectionMsgs; + + try + { + detectionConverter.toRosMsg(inNetData, opDetectionMsgs); + auto rosMsgPtr = detectionConverter.toRosMsgPtr(inNetData); + } + catch (const std::runtime_error &e) + { + // Handle runtime errors separately + const char *errorMsg = e.what(); + for (const char **known = knownErrors; *known != nullptr; ++known) + { + if (strstr(errorMsg, *known) != nullptr) + { + + return 0; + } + } + + throw; + } + + return 0; +} diff --git a/fuzz/imu_conv_fuzz.cc b/fuzz/imu_conv_fuzz.cc new file mode 100644 index 0000000..c1bdf37 --- /dev/null +++ b/fuzz/imu_conv_fuzz.cc @@ -0,0 +1,56 @@ +#include +#include + +#include "depthai-shared/common/CameraInfo.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +extern "C" int LLVMFuzzerTestOneInput(const uint8_t *Data, size_t Size) { + FuzzedDataProvider dataProvider(Data, Size); + + std::string frameName = dataProvider.ConsumeRandomLengthString(); + + auto inData = std::make_shared(); + // NOTE: Starting from 0 leads to an OOB access + size_t numPackets = dataProvider.ConsumeIntegralInRange(1, 10); + + // Generate random packets and add them to inData->packets + for (size_t i = 0; i < numPackets; ++i) { + dai::IMUPacket packet; + + // Populate the fields of IMUPacket + packet.acceleroMeter.x = dataProvider.ConsumeFloatingPoint(); + packet.acceleroMeter.y = dataProvider.ConsumeFloatingPoint(); + packet.acceleroMeter.z = dataProvider.ConsumeFloatingPoint(); + + packet.gyroscope.x = dataProvider.ConsumeFloatingPoint(); + packet.gyroscope.y = dataProvider.ConsumeFloatingPoint(); + packet.gyroscope.z = dataProvider.ConsumeFloatingPoint(); + + packet.magneticField.x = dataProvider.ConsumeFloatingPoint(); + packet.magneticField.y = dataProvider.ConsumeFloatingPoint(); + packet.magneticField.z = dataProvider.ConsumeFloatingPoint(); + + packet.rotationVector.i = dataProvider.ConsumeFloatingPoint(); + packet.rotationVector.j = dataProvider.ConsumeFloatingPoint(); + packet.rotationVector.k = dataProvider.ConsumeFloatingPoint(); + packet.rotationVector.real = dataProvider.ConsumeFloatingPoint(); + packet.rotationVector.rotationVectorAccuracy = + dataProvider.ConsumeFloatingPoint(); + + inData->packets.push_back(packet); + } + + dai::ros::ImuConverter imuConverter(frameName); + dai::ros::ImuMsgs::Imu outImuMsg; + imuConverter.toRosMsg(inData, outImuMsg); + auto rosMsgPtr = imuConverter.toRosMsgPtr(inData); + + return 0; +} diff --git a/include/depthai_ctrl/ImageConverter.hpp b/include/depthai_ctrl/ImageConverter.hpp index 5b9e5a0..dbd3b68 100644 --- a/include/depthai_ctrl/ImageConverter.hpp +++ b/include/depthai_ctrl/ImageConverter.hpp @@ -1,6 +1,6 @@ #pragma once -#include +#include #include #include diff --git a/include/depthai_ctrl/depthai_camera.hpp b/include/depthai_ctrl/depthai_camera.hpp index 962644a..7484ac6 100644 --- a/include/depthai_ctrl/depthai_camera.hpp +++ b/include/depthai_ctrl/depthai_camera.hpp @@ -44,7 +44,7 @@ #include #include #include -#include +#include #include diff --git a/src/ImageConverter.cpp b/src/ImageConverter.cpp index 04249b2..cde747d 100644 --- a/src/ImageConverter.cpp +++ b/src/ImageConverter.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include