diff --git a/.drone.yml b/.drone.yml deleted file mode 100644 index 4538130..0000000 --- a/.drone.yml +++ /dev/null @@ -1,59 +0,0 @@ -kind: pipeline -type: docker -name: build - -platform: - os: linux - arch: amd64 - -steps: - ## Biuld in fips (debug with unittesting) -- name: build-fips-unittest - image: amarburg/lsdslam-dev-host - commands: - - ./fips set config linux-make-unittest - - ./fips build - - ## Build in ros melodic -# - name: build-ros-melodic -# image: amarburg/drone-ci-ros-melodic:latest -# commands: -# - wget -O- https://gitlab.com/amarburg/ros_drone_ci/-/raw/master/bootstrap.sh | /bin/bash -# environment: -# WSTOOL_RECURSIVE: true -# ROS_PACKAGES_TO_INSTALL: cv-bridge image-transport - - ## Build in ros noetic -- name: build-ros-noetic - image: amarburg/drone-ci-ros-noetic:latest - commands: - - wget -O- https://gitlab.com/amarburg/ros_drone_ci/-/raw/master/bootstrap.sh | /bin/bash - environment: - WSTOOL_RECURSIVE: true - ROS_PACKAGES_TO_INSTALL: cv-bridge image-transport - - ## Trigger downstream builds on Github -# - name: trigger-github-downstream -# image: plugins/downstream -# settings: -# server: https://github.drone.camhd.science -# fork: true -# token: -# from_secret: github_drone_token -# repositories: -# - apl-ocean-engineering/serdp_common -# when: -# event: -# exclude: -# - pull_request - -- name: slack - image: plugins/slack - settings: - webhook: - from_secret: slack_webhook - when: - status: [ success, failure ] - event: - exclude: - - pull_request diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000..79fc83a --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,6 @@ +version: 2 +updates: + - package-ecosystem: github-actions + directory: "/" + schedule: + interval: "weekly" diff --git a/.github/mergify.yml b/.github/mergify.yml new file mode 100644 index 0000000..38eb702 --- /dev/null +++ b/.github/mergify.yml @@ -0,0 +1,18 @@ +pull_request_rules: + + - name: ask to resolve conflict + conditions: + - conflict + - author!=mergify[bot] + actions: + comment: + message: This pull request is in conflict. Could you fix it @{{author}}? + + - name: development targets main branch + conditions: + - base!=main + - author!=mergify[bot] + actions: + comment: + message: | + Please target the `main` branch for development, we will backport the changes to {{base}} for you if approved and if they don't break API. diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml new file mode 100644 index 0000000..f92fe45 --- /dev/null +++ b/.github/workflows/ci.yaml @@ -0,0 +1,31 @@ +name: ROS Industrial CI + +on: + push: + branches: + - main + pull_request: + workflow_dispatch: + +jobs: + industrial-ci: + name: Run Industrial CI + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + env: + # We include humble because there are still 22.04 systems (Jetson) out there + - {UPSTREAM_WORKSPACE: sonar_image_proc.repos, ROS_DISTRO: humble} + - {UPSTREAM_WORKSPACE: sonar_image_proc.repos, ROS_DISTRO: kilted} + - {UPSTREAM_WORKSPACE: sonar_image_proc.repos, ROS_DISTRO: rolling} + + steps: + - name: Checkout repository + uses: actions/checkout@v5 + with: + submodules: recursive + + - name: Run ROS Industrial CI + uses: ros-industrial/industrial_ci@master + env: ${{matrix.env}} diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml new file mode 100644 index 0000000..b558fa2 --- /dev/null +++ b/.github/workflows/format.yaml @@ -0,0 +1,34 @@ +name: Formatting (pre-commit) + +on: + pull_request: + push: + branches: + - main + workflow_dispatch: + +jobs: + pre-commit: + name: Format + runs-on: ubuntu-latest + steps: + - name: Checkout repository + uses: actions/checkout@v5 + + - name: Setup Python + uses: actions/setup-python@v6 + with: + python-version: "3.10" + + - name: Install clang-format-14 + run: sudo apt-get install clang-format-14 + + - name: Run pre-commit + uses: pre-commit/action@v3.0.1 + id: precommit + + - name: Upload pre-commit changes + if: failure() && steps.precommit.outcome == 'failure' + uses: rhaschke/upload-git-patch-action@main + with: + name: pre-commit diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index f8d8571..d019757 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,26 +1,43 @@ -# See https://pre-commit.com for more information -# See https://pre-commit.com/hooks.html for more hooks repos: -- repo: https://github.com/pre-commit/pre-commit-hooks - rev: v3.2.0 + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.12.3 hooks: - - id: trailing-whitespace - - id: end-of-file-fixer - - id: check-yaml - - id: check-json - - id: check-xml - - id: check-added-large-files - - id: mixed-line-ending -- repo: https://github.com/pre-commit/mirrors-clang-format - rev: v15.0.4 + # Run the linter + - id: ruff-check + args: ["--fix", "--exit-non-zero-on-fix"] + # Run the formatter. + - id: ruff-format + + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v20.1.7 hooks: - id: clang-format - # Respect .clang-format if it exists, otherwise use Google - args: ["--fallback-style=Google"] -# -# Uncomment the following to enable cpplint -# -#- repo: https://gitlab.com/daverona/pre-commit/cpp -# rev: 0.8.0 -# hooks: -# - id: cpplint + types_or: [c++, c] + + # CMake formatter + - repo: https://github.com/BlankSpruce/gersemi + rev: 0.19.3 + hooks: + - id: gersemi + + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v5.0.0 + hooks: + - id: check-added-large-files + - id: check-case-conflict + - id: check-executables-have-shebangs + - id: check-json + - id: check-merge-conflict + - id: check-shebang-scripts-are-executable + - id: check-symlinks + - id: check-toml + - id: check-xml + - id: check-yaml + - id: debug-statements + - id: destroyed-symlinks + - id: detect-private-key + - id: end-of-file-fixer + - id: fix-byte-order-marker + - id: mixed-line-ending + - id: trailing-whitespace + args: [--markdown-linebreak-ext=md] diff --git a/CMakeLists.txt b/CMakeLists.txt index c8e8987..7e0bc3e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,178 +1,37 @@ -cmake_minimum_required(VERSION 3.5) -project(sonar_image_proc) +cmake_minimum_required(VERSION 3.14.4) +project(libdrawsonar) -if(FIPS_CONFIG AND NOT FIPS_IMPORT) - cmake_minimum_required(VERSION 3.5) - - get_filename_component(FIPS_ROOT_DIR "../fips" ABSOLUTE) - include("${FIPS_ROOT_DIR}/cmake/fips.cmake") - - fips_setup() -else() - find_package(catkin QUIET) -endif() +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +add_compile_options(-std=c++17) +# == Code common to all builds ======================================= list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) -# find_package(Eigen3 REQUIRED ) -find_package(Boost COMPONENTS filesystem system program_options REQUIRED) - -# # Default OpenCV version -list(APPEND OPENCV_COMPONENTS core highgui imgproc) -find_package(OpenCV COMPONENTS ${OPENCV_COMPONENTS} REQUIRED) - -if(FIPS_CONFIG) - # # Set global include paths - fips_include_directories( - ${CMAKE_SOURCE_DIR}/include - ${OpenCV_INCLUDE_DIRS} - ) - - # The local library - fips_begin_module(sonar_image_proc) - - fips_src(lib *.cpp) - fips_libs(${Boost_LIBRARIES} ${OpenCV_LIBS}) - - fips_end_module() - - if(NOT FIPS_IMPORT) - if(FIPS_UNITTESTS) - gtest_begin(sonar_image_proc) - fips_src(test/unit/) - fips_deps(sonar_image_proc) - gtest_end() - endif() - endif() - - fips_finish() - -else() - find_package(catkin REQUIRED - marine_acoustic_msgs - cv_bridge - image_transport - nodelet - nodelet_topic_tools - OpenCV - rosbag_storage - dynamic_reconfigure - ) - - catkin_python_setup() - - # This needs to come after `catkin_python_setup` and before `catkin_package` - generate_dynamic_reconfigure_options( - ros/cfg/DrawSonar.cfg - ) - catkin_package( - INCLUDE_DIRS include/ ros/include/ - LIBRARIES sonar_image_proc - DEPENDS OpenCV - - # CATKIN_DEPENDS - ) - - include_directories( - include/ - ros/include/ - ${catkin_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ) - - set(sonar_image_proc_SRCS +set(drawsonar_SRCS lib/AbstractSonarInterface.cpp lib/ColorMaps.cpp lib/SonarDrawer.cpp lib/OldDrawSonar.cpp lib/HistogramGenerator.cpp - ) - - # "true" library containing core functionality - add_library(sonar_image_proc ${sonar_image_proc_SRCS}) - target_link_libraries(sonar_image_proc ${catkin_LIBRARIES}) - set_property(TARGET sonar_image_proc PROPERTY CXX_STANDARD 14) - add_dependencies(sonar_image_proc ${PROJECT_NAME}_gencfg) - - # Dynamically loadable libraries containing nodelets - add_library(sonar_image_proc_nodelets - ros/src/draw_sonar_nodelet.cpp - ros/src/sonar_postprocessor_nodelet.cpp) - target_link_libraries(sonar_image_proc_nodelets ${catkin_LIBRARIES} sonar_image_proc) - set_property(TARGET sonar_image_proc_nodelets PROPERTY CXX_STANDARD 14) - - # # Standalone nodes - add_executable(draw_sonar_node ros/src/draw_sonar_node.cpp) - target_link_libraries(draw_sonar_node - sonar_image_proc - ${catkin_LIBRARIES}) - add_dependencies(draw_sonar_node ${catkin_EXPORTED_TARGETS}) - set_property(TARGET draw_sonar_node PROPERTY CXX_STANDARD 14) - - add_executable(sonar_postprocessor_node ros/src/sonar_postprocessor_node.cpp) - target_link_libraries(sonar_postprocessor_node - sonar_image_proc - ${catkin_LIBRARIES}) - add_dependencies(sonar_postprocessor_node ${catkin_EXPORTED_TARGETS}) - set_property(TARGET sonar_postprocessor_node PROPERTY CXX_STANDARD 14) - - # bag2sonar is **not** a conventional ROS node, but a standalone - # executable which uses ros_storage to read ros bags - add_executable(bag2sonar ros/tools/bag2sonar.cpp) - target_link_libraries(bag2sonar - sonar_image_proc - ${catkin_LIBRARIES}) - add_dependencies(bag2sonar ${catkin_EXPORTED_TARGETS}) - set_property(TARGET bag2sonar PROPERTY CXX_STANDARD 14) - - install(TARGETS draw_sonar_node - sonar_postprocessor_node - bag2sonar - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - - install(TARGETS - sonar_image_proc_nodelets - sonar_image_proc - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - - install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) - - # install( DIRECTORY rqt_config/ - # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rqt_config ) - install(FILES nodelet_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - - # # Install headers - install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.hpp" PATTERN "*.h" - PATTERN ".git" EXCLUDE) - - install(DIRECTORY ros/include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.hpp" PATTERN "*.h" - PATTERN ".git" EXCLUDE) - - # Install python scripts and modules - catkin_install_python(PROGRAMS - scripts/sonar_pointcloud.py - scripts/sonar_fov.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - - # # Disable python wrapper for now ... - # - # # == Stuff related to the Python wrapper == - # pybind_add_module(py_draw_sonar SHARED - # python/py_draw_sonar.cpp - # python/ndarray_converter.cpp - # lib/DrawSonar.cpp - # lib/DataStructures.cpp - # lib/ColorMaps.cpp - # ) - # target_link_libraries( py_draw_sonar ${OpenCV_LIBS} draw_sonar ) - # - # install(TARGETS py_draw_sonar - # DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION} ) #${CATKIN_PACKAGE_PYTHON_DESTINATION}) +) + +# ROS build, determine which version... +if(DEFINED ENV{ROS_VERSION}) + find_package(ros_environment) + + set(ROS_VERSION $ENV{ROS_VERSION}) + + if(${ROS_VERSION} EQUAL 2) + include(cmake/BuildROS2.cmake) + elseif(${ROS_VERSION} EQUAL 1) + include(cmake/BuildROS1.cmake) + else() + message( + "Unsure what sort of build to do. Not in a ROS1 or ROS2 environment" + ) + endif() +else() + message(NOTICE "!! Performing non-ROS build !!") + include(cmake/BuildNonROS.cmake) endif() diff --git a/README.md b/README.md index b2501a1..4e48a8c 100644 --- a/README.md +++ b/README.md @@ -1,118 +1,8 @@ # sonar_image_proc -[![pre-commit](https://img.shields.io/badge/pre--commit-enabled-brightgreen?logo=pre-commit)](https://github.com/pre-commit/pre-commit) - -Code to draw data from forward-looking imaging sonars. - -If built for ROS, it will build a node/nodelet -[draw_sonar](https://github.com/apl-ocean-engineering/libdraw_sonar/tree/master/src_ros) -which subscribes to an -[marine_acoustic_msgs/ProjectedSonarImage](https://github.com/apl-ocean-engineering/marine_msgs/blob/main/marine_acoustic_msgs/msg/ProjectedSonarImage.msg) -and publishes a -[sensor_msgs/Image](https://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Image.html). - -The core library contains no ROS dependencies, and can be linked into non-ROS applications. - -# ROS Interfaces (draw_sonar_node) - -`rosrun sonar_image_proc draw_sonar_node` - -## Subscribers - -Subscribes to the topic `sonar_image` of type [marine_acoustic_msgs/ProjectedSonarImage](https://github.com/apl-ocean-engineering/marine_msgs/blob/main/marine_acoustic_msgs/msg/ProjectedSonarImage.msg). - - -## Publishers - -By default publishes three [sensor_msgs/Image](https://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Image.html) topics: - -* `drawn_sonar` contains an image of the sonar drawn with a cartesian projection -with the origin (range = 0) of the sonar centered on the bottom edge of -the image, and azimuth=0 extending vertically upwards in the image. By default, -the image height is set by the number range bins, and the image width is -automatically determined based on the height and the min/max azimuth of the -sonar image. The color map used to convert the sonar intensity to RGB is set -in code. - -![](drawn_sonar.png) - -* `drawn_sonar_rect` is the contents of the SonarImage data mapped directly from polar to cartesian coordinates. -Since the intensity data in the SonarImage is stored azimuth-major, the data is -mapped into the image space with range in the X direction and azimuth in the Y -direction: - - * Image width is the number of range bins in the data, with the minimum range - on the left side and maximum range on the right side. - - * Image height is the number of azimuth bins in the data, with the lowest - azimuth (typically the most negative) at the top, and most positive at the - bottom. - -![](drawn_sonar_rect.png) - -* `drawn_sonar_osd` adds guidelines and annotations to `drawn_sonar`. Overlay parameters can be configured in realtime. - -If the param `publish_timing` is `true`, the node will track the elapsed time to -draw each sonar image and publish that information to the topic `sonar_image_proc_timing` -as a [std_msgs/String](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/String.html) -containing a JSON dict. - -If the param `publish_old` is `true`, the node will also draw the sonar using -the old `draw_sonar` algorithm for comparison. - -## Params - -If `max_range` is set to a non-zero value, images will be clipped/dropped to that max range (or the actual sonar range, whichever is smaller). - -If `publish_timing` is `true` the node will publish performance information as a -JSON [string](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/String.html) -to the topic `sonar_image_proc_timing`. Defaults to `true` - -If `publish_old` is `true` the node will also draw the sonar using the old -algorithm to the topic `old_drawn_sonar`. Defaults to `false` - -If `publish_histogram` is `true` the node will publish a "raw" histogram information as a `UInt32MultiArray` to the topic `histogram`. It contains a vector of unsigned ints giving the count for each intensity value -- so for 8 bit data the vector will be 256 elements in length, and for 16-bit data it will be 65536 elements in length. - -# bag2sonar - -The program `bag2sonar` reads in a bagfile containing a `ProjectedSonarImage` topic, draws the sonar image and writes those images to *new* bagfile in a `Image` topic. - -Usage: - -``` -$ rosrun sonar_image_proc bag2sonar -Usage: - - bag2sonar [options] - -Draw sonar from a bagfile: - -h [ --help ] Display this help message - -l [ --logscale ] Do logscale - --min-db arg (=0) Min db - --max-db arg (=0) Max db - --osd If set, include the on-screen display - in output - -o [ --output-bag ] arg Name of output bagfile - -t [ --output-topic ] arg (=/drawn_sonar) - Topic for images in output bagfile -``` - -Note that `bag2sonar` is not a conventional ROS node, it is intended to run as a standalone commandline program. It uses `ros_storage` to read the input bagfile sequentially, rather than subscribing to a topic. - -# histogram_drawer - -`python/histogram_drawer` is a Python script which subscribes to the `histogram` topic and uses numpy+Matplotlib to bin the data (into a fixed set of 128 bin right now), and draw a plot to the topic `drawn_histogram`. - -# Python API - -Long term, I'd like to be able to call this drawing function from Python, -however we're not there yet. - -There IS a totally separate python node that publishes a pointcloud -for visualization in rviz: - -`rosrun sonar_image_proc sonar_pointcloud.py` +Library to draw data from forward-looking imaging sonars. +This repo is designed to build in Colcon (ROS2), Catkin (ROS1) and cmake environments, but it contains **no** ROS-specific code. ROS1,2 wrappers are found in [sonar_image_proc](https://github.com/apl-ocean-engineering/sonar_image_proc) # API @@ -122,6 +12,7 @@ A convenience function [drawSonar](include/sonar_image_proc/DrawSonar.h) is also # Related Packages +* [sonar_image_proc](https://github.com/apl-ocean-engineering/sonar_image_proc) provides ROS{1,2} nodes for creating images from sonar data. * [liboculus](https://github.com/apl-ocean-engineering/liboculus) provides network IO and data parsing for the Oculus sonar (non-ROS). * [oculus_sonar_driver](https://gitlab.com/apl-ocean-engineering/oculus_sonar_driver) provides a ROS node for interfacing with the Oculus sonar. * [marine_acoustic_msgs](https://github.com/apl-ocean-engineering/marine_msgs/blob/main/marine_acoustic_msgs) defines the ROS [ProjectedSonarImage](https://github.com/apl-ocean-engineering/marine_msgs/blob/main/marine_acoustic_msgs/msg/ProjectedSonarImage.msg) message type published by [oculus_sonar_driver](https://gitlab.com/apl-ocean-engineering/oculus_sonar_driver). diff --git a/cmake/BuildNonROS.cmake b/cmake/BuildNonROS.cmake new file mode 100644 index 0000000..def4b02 --- /dev/null +++ b/cmake/BuildNonROS.cmake @@ -0,0 +1,98 @@ +# Specify the minimum version for CMake +cmake_minimum_required(VERSION 3.12) +project(libdrawsonar) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -march=native -Wl,--no-as-needed") + +# Set the output folder where your program will be created +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/bin) +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/lib) + +# ########################################### +# The following folders will be included # +# ########################################### +include_directories("${PROJECT_SOURCE_DIR}/include/") + +find_package(spdlog) +find_package(OpenCV COMPONENTS core imgproc) + +include_directories(${install_dir}/include/) + +# ##################### +# Add Execuatables # +# ##################### +link_directories(${Boost_LIBRARY_DIRS}) + +# Create Library +add_library(drawsonar SHARED ${drawsonar_SRCS}) +set_target_properties(drawsonar PROPERTIES LIBRARY_OUTPUT_NAME drawsonar) +target_link_libraries( + drawsonar + PUBLIC spdlog::spdlog opencv_core opencv_imgproc opencv_highgui +) + +# ============================================= +# to allow find_package() +# ============================================= +# +# The following is borrowed heavily from: +# https://github.com/RossHartley/invariant-ekf +# I am responsible for all mistakes +# +# the following case be used in an external project requiring drawsonar: +# ... +# find_package(drawsonar) +# include_directories(${drawsonar_INCLUDE_DIRS}) +# ... + +# NOTE: the following will support find_package for 1) local build (make) and 2) for installed files (make install) + +# 1- local build + +# Register the local build in case one doesn't use "make install" +export(PACKAGE drawsonar) + +# Create variable for the local build tree +# set_target_properties(drawsonar PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_LIBRARY_OUTPUT_DIRECTORY}) +get_property( + drawsonar_include_dirs + DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + PROPERTY INCLUDE_DIRECTORIES +) +get_property( + drawsonar_library_dirs + TARGET drawsonar + PROPERTY LIBRARY_OUTPUT_DIRECTORY +) +get_property(drawsonar_libraries TARGET drawsonar PROPERTY LIBRARY_OUTPUT_NAME) + +message("drawsonar_include_dirs: " ${drawsonar_include_dirs}) +message("drawsonar_library_dirs: " ${drawsonar_library_dirs}) +message("drawsonar_libraries: " ${drawsonar_libraries}) + +# Configure config file for local build tree +configure_file( + cmake/drawsonarConfig.cmake.in + "${PROJECT_BINARY_DIR}/drawsonarConfig.cmake" + @ONLY +) + +message("PROJECT_BINARY_DIR: " ${PROJECT_BINARY_DIR}) + +# 2- installation build # + +# Change the include location for the case of an install location +set(drawsonar_include_dirs ${CMAKE_INSTALL_PREFIX}/include ${EIGEN_INCLUDE_DIR}) + +# We put the generated file for installation in a different repository (i.e., ./CMakeFiles/) +configure_file( + cmake/drawsonarConfig.cmake.in + "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/drawsonarConfig.cmake" + @ONLY +) + +install( + FILES "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/drawsonarConfig.cmake" + DESTINATION share/drawsonar/cmake + COMPONENT dev +) diff --git a/cmake/BuildROS1.cmake b/cmake/BuildROS1.cmake new file mode 100644 index 0000000..0da0fdc --- /dev/null +++ b/cmake/BuildROS1.cmake @@ -0,0 +1,45 @@ +# Catkin/ROS1 section ===== + +catkin_package( + INCLUDE_DIRS include + LIBRARIES libdrawsonar +) + +add_library(libdrawsonar ${drawsonar_SRCS}) + +include_directories(libdrawsonar include ${catkin_INCLUDE_DIRS}) + +target_link_libraries(libdrawsonar ${catkin_LIBRARIES}) + +install( + TARGETS libdrawsonar + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +## Install headers +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING + PATTERN "*.hpp" + PATTERN "*.h" + PATTERN ".git" EXCLUDE +) + +if(CATKIN_ENABLE_TESTING) + add_definitions(-DTEST_DATA_PATH="${CMAKE_CURRENT_SOURCE_DIR}/test/data") + include_directories(test/data/) + + file(GLOB drawsonar_test_SRCS test/unit/*cpp) + + catkin_add_gtest(drawsonar_test ${drawsonar_test_SRCS}) + + target_link_libraries( + drawsonar_test + ${catkin_LIBRARIES} + libdrawsonar + Boost::system + ) +endif() diff --git a/cmake/BuildROS2.cmake b/cmake/BuildROS2.cmake new file mode 100644 index 0000000..8e12f31 --- /dev/null +++ b/cmake/BuildROS2.cmake @@ -0,0 +1,36 @@ +# == ament/ROS2 section ================================= + +find_package(ament_cmake REQUIRED) +find_package(OpenCV REQUIRED) + +add_library(drawsonar SHARED ${drawsonar_SRCS}) +target_link_libraries(drawsonar PUBLIC opencv_core) + +target_include_directories( + drawsonar + PUBLIC + "$" + "$" +) + +install( + TARGETS drawsonar + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include + FILES_MATCHING + PATTERN "*.hpp" + PATTERN "*.h" + PATTERN ".git" EXCLUDE +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_libraries(drawsonar) + +ament_package() diff --git a/cmake/drawsonarConfig.cmake.in b/cmake/drawsonarConfig.cmake.in new file mode 100644 index 0000000..2e22029 --- /dev/null +++ b/cmake/drawsonarConfig.cmake.in @@ -0,0 +1,10 @@ +# - Config file for the drawsonar package +# It defines the following variables +# drawsonar_INCLUDE_DIRS - include directories for drawsonar +# drawsonar_LIBRARY_DIRS - directories for drawsonar library + +# Compute paths +get_filename_component(drawsonar_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) +set(drawsonar_INCLUDE_DIRS "@drawsonar_include_dirs@") +set(drawsonar_LIBRARY_DIRS "@drawsonar_library_dirs@") +set(drawsonar_LIBRARIES "@drawsonar_libraries@") diff --git a/fips b/fips deleted file mode 100755 index efdeb88..0000000 --- a/fips +++ /dev/null @@ -1,17 +0,0 @@ -#!/usr/bin/env python -"""fips main entry""" -import os -import sys -import subprocess -proj_path = os.path.dirname(os.path.abspath(__file__)) -fips_path = os.path.dirname(proj_path) + '/fips' -if not os.path.isdir(fips_path) : - print("\033[93m=== cloning fips build system to '{}':\033[0m".format(fips_path)) - subprocess.call(['git', 'clone', 'https://github.com/amarburg/fips.git', fips_path]) -sys.path.insert(0,fips_path) -try : - from mod import fips -except ImportError : - print("\033[91m[ERROR]\033[0m failed to initialize fips build system in '{}'".format(proj_path)) - sys.exit(10) -fips.run(fips_path, proj_path, sys.argv) diff --git a/fips.yml b/fips.yml deleted file mode 100644 index ede8bb8..0000000 --- a/fips.yml +++ /dev/null @@ -1,15 +0,0 @@ -imports: - fips-googletest: - git: https://github.com/amarburg/fips-googletest.git - cond: "FIPS_UNITTESTS" - -exports: - modules: - draw_sonar: . - header-dirs: - - include - -defines: - FIPS_EXCEPTIONS: ON - FIPS_RTTI: ON - FIPS_UNITTESTS_RUN_AFTER_BUILD: ON diff --git a/helpers/make_color_map.py b/helpers/make_color_map.py index 0cad913..f1f4a4a 100755 --- a/helpers/make_color_map.py +++ b/helpers/make_color_map.py @@ -5,16 +5,22 @@ import numpy as np if __name__ == "__main__": - - parser = argparse.ArgumentParser(description='Make .cpp source file for a colormap') - - parser.add_argument('--prologue', type=Path, default="prologue.txt", - help='Test file containing boilerplate for the beginning of file') - parser.add_argument('--epilogue', type=Path, default='epilogue.txt', - help="Test file containing boilerplate for end of file") - - parser.add_argument('--csv', type=Path, nargs="*", - help='CSV file containing') + parser = argparse.ArgumentParser(description="Make .cpp source file for a colormap") + + parser.add_argument( + "--prologue", + type=Path, + default="prologue.txt", + help="Test file containing boilerplate for the beginning of file", + ) + parser.add_argument( + "--epilogue", + type=Path, + default="epilogue.txt", + help="Test file containing boilerplate for end of file", + ) + + parser.add_argument("--csv", type=Path, nargs="*", help="CSV file containing") args = parser.parse_args() @@ -24,23 +30,23 @@ print(line.strip()) for csv_file in args.csv: - data = np.loadtxt(csv_file, delimiter=',', dtype=float) + data = np.loadtxt(csv_file, delimiter=",", dtype=float) print("const float ColorMap::float_data[%d][3] = {" % len(data)) for entry in data: - print("{%f,%f,%f}," % (entry[0],entry[1],entry[2])) + print("{%f,%f,%f}," % (entry[0], entry[1], entry[2])) print("};") print() print("const float ColorMap::char_data[%d][3] = {" % len(data)) for entry in data: - print("{%d,%d,%d}," % (int(entry[0]*255), - int(entry[1]*255), - int(entry[2]*255))) + print( + "{%d,%d,%d}," + % (int(entry[0] * 255), int(entry[1] * 255), int(entry[2] * 255)) + ) print("};") print() - if args.epilogue: with open(args.epilogue) as fp: for line in fp: diff --git a/include/sonar_image_proc/AbstractSonarInterface.h b/include/libdrawsonar/AbstractSonarInterface.h similarity index 96% rename from include/sonar_image_proc/AbstractSonarInterface.h rename to include/libdrawsonar/AbstractSonarInterface.h index a5eff34..abb98fb 100644 --- a/include/sonar_image_proc/AbstractSonarInterface.h +++ b/include/libdrawsonar/AbstractSonarInterface.h @@ -10,7 +10,7 @@ #include #include -namespace sonar_image_proc { +namespace libdrawsonar { typedef std::pair Bounds_t; extern const Bounds_t UnsetBounds; @@ -28,7 +28,7 @@ struct AzimuthRangeIndices { // Designed as a "common abstact type" between the Blueprint // Subsea SimplePingResult and ROS ImagingSonarMsg struct AbstractSonarInterface { - public: +public: AbstractSonarInterface(); enum DataType_t { @@ -54,7 +54,7 @@ struct AbstractSonarInterface { int nAzimuth() const { return azimuths().size(); } int nAzimuths() const { return azimuths().size(); - } // Whoops, should be consistent + } // Whoops, should be consistent float azimuth(int n) const { return azimuths().at(n); } Bounds_t azimuthBounds() const; @@ -128,13 +128,13 @@ struct AbstractSonarInterface { } __attribute__((deprecated)); - private: +private: // In a few cases, need to "check and potentially calculate cached // value" without actually getting the value void checkRangeBounds() const; void checkAzimuthBounds() const; - private: +private: // Since we search extensively for the bounds // (rather than assuming the first and last are the bounds), // cache the results @@ -143,4 +143,4 @@ struct AbstractSonarInterface { mutable float _maxRangeSquared; }; -} // namespace sonar_image_proc +} // namespace libdrawsonar diff --git a/include/sonar_image_proc/ColorMaps.h b/include/libdrawsonar/ColorMaps.h similarity index 95% rename from include/sonar_image_proc/ColorMaps.h rename to include/libdrawsonar/ColorMaps.h index f098dba..fc17ccf 100644 --- a/include/sonar_image_proc/ColorMaps.h +++ b/include/libdrawsonar/ColorMaps.h @@ -5,11 +5,11 @@ #include -#include "sonar_image_proc/AbstractSonarInterface.h" +#include "libdrawsonar/AbstractSonarInterface.h" -namespace sonar_image_proc { +namespace libdrawsonar { -using sonar_image_proc::AbstractSonarInterface; +using libdrawsonar::AbstractSonarInterface; struct SonarColorMap { // A ColorMap is a mapping from one pixel of sonar data to @@ -106,4 +106,4 @@ struct InfernoSaturationColorMap : public InfernoColorMap { } }; -} // namespace sonar_image_proc +} // namespace libdrawsonar diff --git a/include/sonar_image_proc/DataStructures.h b/include/libdrawsonar/DataStructures.h similarity index 84% rename from include/sonar_image_proc/DataStructures.h rename to include/libdrawsonar/DataStructures.h index 36fc720..a1cf07f 100644 --- a/include/sonar_image_proc/DataStructures.h +++ b/include/libdrawsonar/DataStructures.h @@ -8,7 +8,7 @@ #include -namespace sonar_image_proc { +namespace libdrawsonar { // \todo. Are these used anymore? struct SonarPoint { @@ -19,4 +19,4 @@ struct SonarPoint { SonarPoint bearingRange2Cartesian(float bearing, float range); -} // namespace sonar_image_proc +} // namespace libdrawsonar diff --git a/include/sonar_image_proc/DrawSonar.h b/include/libdrawsonar/DrawSonar.h similarity index 73% rename from include/sonar_image_proc/DrawSonar.h rename to include/libdrawsonar/DrawSonar.h index 74e1538..9f60565 100644 --- a/include/sonar_image_proc/DrawSonar.h +++ b/include/libdrawsonar/DrawSonar.h @@ -11,13 +11,13 @@ #include #include -#include "sonar_image_proc/AbstractSonarInterface.h" -#include "sonar_image_proc/ColorMaps.h" -#include "sonar_image_proc/SonarDrawer.h" +#include "libdrawsonar/AbstractSonarInterface.h" +#include "libdrawsonar/ColorMaps.h" +#include "libdrawsonar/SonarDrawer.h" -namespace sonar_image_proc { +namespace libdrawsonar { -inline cv::Mat drawSonar(const sonar_image_proc::AbstractSonarInterface &ping, +inline cv::Mat drawSonar(const libdrawsonar::AbstractSonarInterface &ping, const SonarColorMap &colorMap = InfernoColorMap(), const cv::Mat &image = cv::Mat(0, 0, CV_8UC3)) { SonarDrawer drawer; @@ -38,10 +38,10 @@ inline cv::Mat drawSonar(const sonar_image_proc::AbstractSonarInterface &ping, // // Cell (nRange,nBearing) is the data at the max range, most positive bearing // -inline cv::Mat drawSonarRectImage( - const sonar_image_proc::AbstractSonarInterface &ping, - const SonarColorMap &colorMap = InfernoColorMap(), - const cv::Mat &rectImage = cv::Mat(0, 0, CV_8UC3)) { +inline cv::Mat +drawSonarRectImage(const libdrawsonar::AbstractSonarInterface &ping, + const SonarColorMap &colorMap = InfernoColorMap(), + const cv::Mat &rectImage = cv::Mat(0, 0, CV_8UC3)) { SonarDrawer drawer; return drawer.drawRectSonarImage(ping, colorMap, rectImage); } @@ -61,15 +61,15 @@ namespace old_api { // // If set, maxRange is used in lieu of the ping's native max range, // allowing truncation of the image -cv::Size calculateImageSize( - const sonar_image_proc::AbstractSonarInterface &ping, cv::Size hint, - int pixPerRangeBin = 2, float maxRange = -1.0); +cv::Size calculateImageSize(const libdrawsonar::AbstractSonarInterface &ping, + cv::Size hint, int pixPerRangeBin = 2, + float maxRange = -1.0); -cv::Mat drawSonar(const sonar_image_proc::AbstractSonarInterface &ping, +cv::Mat drawSonar(const libdrawsonar::AbstractSonarInterface &ping, cv::Mat &mat, const SonarColorMap &colorMap = InfernoColorMap(), float maxRange = -1.0); -} // namespace old_api +} // namespace old_api -} // namespace sonar_image_proc +} // namespace libdrawsonar diff --git a/include/libdrawsonar/HistogramGenerator.h b/include/libdrawsonar/HistogramGenerator.h new file mode 100644 index 0000000..9bb1d98 --- /dev/null +++ b/include/libdrawsonar/HistogramGenerator.h @@ -0,0 +1,24 @@ +// Copyright 2021 University of Washington Applied Physics Laboratory +// + +#pragma once + +#include "libdrawsonar/AbstractSonarInterface.h" + +namespace libdrawsonar { + +class HistogramGenerator { +public: + static std::vector Generate(const AbstractSonarInterface &ping); + + static std::vector + GenerateUint8(const AbstractSonarInterface &ping); + static std::vector + GenerateUint16(const AbstractSonarInterface &ping); + + // Histogram generator for 32bit data produces 256 bins of log10(intensity) + static std::vector + GenerateUint32(const AbstractSonarInterface &ping); +}; + +} // namespace libdrawsonar diff --git a/include/libdrawsonar/Logger.h b/include/libdrawsonar/Logger.h new file mode 100644 index 0000000..a0ecb2d --- /dev/null +++ b/include/libdrawsonar/Logger.h @@ -0,0 +1,145 @@ + +/* + * Copyright (c) 2017-2025 University of Washington + * Author: Aaron Marburg + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of University of Washington nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include +#include + +namespace libdrawsonar { + +class Logger { +public: + static std::shared_ptr & + get_instance(const std::shared_ptr &logger_in = nullptr) { + static std::shared_ptr s_logger(init(logger_in)); + if ((logger_in) && (logger_in != s_logger->logger_)) + s_logger->logger_ = logger_in; + return s_logger; + } + + static std::shared_ptr &get_logger() { + return Logger::get_instance()->logger_; + } + + static void set_logger(const std::shared_ptr &s) { + Logger::get_instance(s); + } + + static void add_sink(const spdlog::sink_ptr &s) { + // n.b. per the spdlog documentation, this is _not_ thread safe + Logger::get_logger()->sinks().push_back(s); + } + + ~Logger() {}; + +private: + static std::shared_ptr + init(const std::shared_ptr &logger_in = nullptr) { + // Use new to access private constructor + return std::shared_ptr(new Logger(logger_in)); + } + + Logger(const std::shared_ptr &l = nullptr) : logger_(l) { + if (!logger_) { + logger_ = std::make_shared("libdrawsonar"); + spdlog::register_logger(logger_); + } + }; + + std::shared_ptr logger_; + + Logger(const Logger &) = delete; + Logger &operator=(const Logger &) = delete; +}; + +// Convenience wrappers around "oclog::" +namespace oclog { +using spdlog::format_string_t; +using spdlog::source_loc; + +template +inline void log(source_loc source, spdlog::level::level_enum lvl, + format_string_t fmt, Args &&...args) { + Logger::get_logger()->log(source, lvl, fmt, std::forward(args)...); +} + +template +inline void log(spdlog::level::level_enum lvl, format_string_t fmt, + Args &&...args) { + Logger::get_logger()->log(source_loc{}, lvl, fmt, + std::forward(args)...); +} + +template +inline void trace(format_string_t fmt, Args &&...args) { + Logger::get_logger()->trace(fmt, std::forward(args)...); +} + +template +inline void debug(format_string_t fmt, Args &&...args) { + Logger::get_logger()->debug(fmt, std::forward(args)...); +} + +template +inline void info(format_string_t fmt, Args &&...args) { + Logger::get_logger()->info(fmt, std::forward(args)...); +} + +template +inline void warn(format_string_t fmt, Args &&...args) { + Logger::get_logger()->warn(fmt, std::forward(args)...); +} + +template +inline void error(format_string_t fmt, Args &&...args) { + Logger::get_logger()->error(fmt, std::forward(args)...); +} + +template +inline void critical(format_string_t fmt, Args &&...args) { + Logger::get_logger()->critical(fmt, std::forward(args)...); +} + +template +inline void log(source_loc source, spdlog::level::level_enum lvl, + const T &msg) { + Logger::get_logger()->log(source, lvl, msg); +} + +template +inline void log(spdlog::level::level_enum lvl, const T &msg) { + Logger::get_logger()->log(lvl, msg); +} +} // namespace oclog + +} // namespace libdrawsonar diff --git a/include/sonar_image_proc/OverlayImage.h b/include/libdrawsonar/OverlayImage.h similarity index 98% rename from include/sonar_image_proc/OverlayImage.h rename to include/libdrawsonar/OverlayImage.h index 14df320..c26d50a 100644 --- a/include/sonar_image_proc/OverlayImage.h +++ b/include/libdrawsonar/OverlayImage.h @@ -1,7 +1,7 @@ #include #include -namespace sonar_image_proc { +namespace libdrawsonar { using cv::Mat; using cv::Vec; @@ -111,4 +111,4 @@ void overlayImage(const Mat &bg, const Mat &fg, Mat &dst) { // // } // // } // } -}; // namespace sonar_image_proc +}; // namespace libdrawsonar diff --git a/include/sonar_image_proc/SonarDrawer.h b/include/libdrawsonar/SonarDrawer.h similarity index 86% rename from include/sonar_image_proc/SonarDrawer.h rename to include/libdrawsonar/SonarDrawer.h index 63b4a93..f6a317c 100644 --- a/include/sonar_image_proc/SonarDrawer.h +++ b/include/libdrawsonar/SonarDrawer.h @@ -10,35 +10,35 @@ #include #include -#include "sonar_image_proc/AbstractSonarInterface.h" -#include "sonar_image_proc/ColorMaps.h" +#include "libdrawsonar/AbstractSonarInterface.h" +#include "libdrawsonar/ColorMaps.h" -namespace sonar_image_proc { +namespace libdrawsonar { -using sonar_image_proc::AbstractSonarInterface; +using libdrawsonar::AbstractSonarInterface; // Function in lib/OverlayImage.cpp void overlayImage(const cv::Mat &background, const cv::Mat &foreground, cv::Mat &output); -#define OVERLAY_RW(var, tp, set, get) \ - OverlayConfig &set(tp i) { \ - var = i; \ - return *this; \ - } \ +#define OVERLAY_RW(var, tp, set, get) \ + OverlayConfig &set(tp i) { \ + var = i; \ + return *this; \ + } \ tp get() const { return var; } class SonarDrawer { - public: +public: struct OverlayConfig { - public: + public: int DEFAULT_LINE_THICKNESS = 1; float DEFAULT_LINE_ALPHA = 0.5; // range spacing of 0 means "calculate automatically" float DEFAULT_RANGE_SPACING = 0; - float DEFAULT_RADIAL_SPACING = 20; // degrees + float DEFAULT_RADIAL_SPACING = 20; // degrees bool DEFAULT_RADIAL_AT_ZERO = false; float DEFAULT_FONT_SCALE = 1.0; @@ -49,8 +49,7 @@ class SonarDrawer { range_spacing_m_(DEFAULT_RANGE_SPACING), radial_spacing_deg_(DEFAULT_RADIAL_SPACING), radial_at_zero_(DEFAULT_RADIAL_AT_ZERO), - font_scale_(DEFAULT_FONT_SCALE), - line_color_(255, 255, 255) {} + font_scale_(DEFAULT_FONT_SCALE), line_color_(255, 255, 255) {} OVERLAY_RW(line_alpha_, float, setLineAlpha, lineAlpha) OVERLAY_RW(line_thickness_, int, setLineThickness, lineThickness) @@ -76,7 +75,7 @@ class SonarDrawer { (lineColor() != other.lineColor()); } - private: + private: int line_thickness_; float line_alpha_; float range_spacing_m_; @@ -125,7 +124,7 @@ class SonarDrawer { OverlayConfig &overlayConfig() { return overlay_config_; } - private: +private: OverlayConfig overlay_config_; // Utility class which can generate and store the two cv::Mats @@ -134,10 +133,10 @@ class SonarDrawer { // Also stores meta-information to determine if the map is // invalid and needs to be regenerated. struct Cached { - public: + public: Cached() { ; } - protected: + protected: virtual bool isValid(const AbstractSonarInterface &ping) const; // Meta-information to validate map @@ -146,13 +145,13 @@ class SonarDrawer { }; struct CachedMap : public Cached { - public: + public: CachedMap() : Cached() { ; } typedef std::pair MapPair; MapPair operator()(const AbstractSonarInterface &ping); - private: + private: bool isValid(const AbstractSonarInterface &ping) const override; void create(const AbstractSonarInterface &ping); @@ -160,14 +159,14 @@ class SonarDrawer { } _map; struct CachedOverlay : public Cached { - public: + public: CachedOverlay() : Cached() { ; } const cv::Mat &operator()(const AbstractSonarInterface &ping, const cv::Mat &sonarImage, const OverlayConfig &config); - private: + private: bool isValid(const AbstractSonarInterface &ping, const cv::Mat &sonarImage, const OverlayConfig &config) const; @@ -179,6 +178,6 @@ class SonarDrawer { } _overlay; -}; // namespace sonar_image_procclassSonarDrawer +}; // namespace classSonarDrawer -} // namespace sonar_image_proc +} // namespace libdrawsonar diff --git a/include/sonar_image_proc/HistogramGenerator.h b/include/sonar_image_proc/HistogramGenerator.h deleted file mode 100644 index 1bbcb47..0000000 --- a/include/sonar_image_proc/HistogramGenerator.h +++ /dev/null @@ -1,24 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// - -#pragma once - -#include "sonar_image_proc/AbstractSonarInterface.h" - -namespace sonar_image_proc { - -class HistogramGenerator { - public: - static std::vector Generate(const AbstractSonarInterface &ping); - - static std::vector GenerateUint8( - const AbstractSonarInterface &ping); - static std::vector GenerateUint16( - const AbstractSonarInterface &ping); - - // Histogram generator for 32bit data produces 256 bins of log10(intensity) - static std::vector GenerateUint32( - const AbstractSonarInterface &ping); -}; - -} // namespace sonar_image_proc diff --git a/launch/sonar_postproc.launch b/launch/sonar_postproc.launch deleted file mode 100644 index 48ea524..0000000 --- a/launch/sonar_postproc.launch +++ /dev/null @@ -1,50 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/lib/AbstractSonarInterface.cpp b/lib/AbstractSonarInterface.cpp index 9c72024..e09e86c 100644 --- a/lib/AbstractSonarInterface.cpp +++ b/lib/AbstractSonarInterface.cpp @@ -1,12 +1,12 @@ // Copyright 2021 University of Washington Applied Physics Laboratory // -#include "sonar_image_proc/AbstractSonarInterface.h" +#include "libdrawsonar/AbstractSonarInterface.h" #include #include -namespace sonar_image_proc { +namespace libdrawsonar { const Bounds_t UnsetBounds = Bounds_t(-1, -1); @@ -42,4 +42,4 @@ void AbstractSonarInterface::checkAzimuthBounds() const { } } -} // namespace sonar_image_proc +} // namespace libdrawsonar diff --git a/lib/ColorMaps.cpp b/lib/ColorMaps.cpp index b7be98a..687c7a8 100644 --- a/lib/ColorMaps.cpp +++ b/lib/ColorMaps.cpp @@ -8,9 +8,9 @@ // // This file autogenerated with helpers/make_color_map.py -#include "sonar_image_proc/ColorMaps.h" +#include "libdrawsonar/ColorMaps.h" -namespace sonar_image_proc { +namespace libdrawsonar { const float InfernoColorMap::_inferno_data_float[256][3] = { {0.001462, 0.000466, 0.013866}, {0.002267, 0.001270, 0.018570}, @@ -210,4 +210,4 @@ const float InfernoColorMap::_inferno_data_uint8[256][3] = { {247, 251, 153}, {249, 252, 157}, {250, 253, 160}, {252, 254, 164}, }; -} // namespace sonar_image_proc +} // namespace libdrawsonar diff --git a/lib/HistogramGenerator.cpp b/lib/HistogramGenerator.cpp index a82e9aa..ff4ebc5 100644 --- a/lib/HistogramGenerator.cpp +++ b/lib/HistogramGenerator.cpp @@ -1,14 +1,14 @@ // Copyright 2021 University of Washington Applied Physics Laboratory // -#include "sonar_image_proc/HistogramGenerator.h" +#include "libdrawsonar/HistogramGenerator.h" #include -namespace sonar_image_proc { +namespace libdrawsonar { -std::vector HistogramGenerator::Generate( - const AbstractSonarInterface &ping) { +std::vector +HistogramGenerator::Generate(const AbstractSonarInterface &ping) { if (ping.data_type() == AbstractSonarInterface::TYPE_UINT8) return HistogramGenerator::GenerateUint8(ping); else if (ping.data_type() == AbstractSonarInterface::TYPE_UINT16) @@ -20,8 +20,8 @@ std::vector HistogramGenerator::Generate( } // \todo Some repetition, but these functions are pretty simple -std::vector HistogramGenerator::GenerateUint8( - const AbstractSonarInterface &ping) { +std::vector +HistogramGenerator::GenerateUint8(const AbstractSonarInterface &ping) { std::vector result(256, 0); for (int r = 0; r < ping.nRanges(); r++) { @@ -34,8 +34,8 @@ std::vector HistogramGenerator::GenerateUint8( return result; } -std::vector HistogramGenerator::GenerateUint16( - const AbstractSonarInterface &ping) { +std::vector +HistogramGenerator::GenerateUint16(const AbstractSonarInterface &ping) { std::vector result(65536, 0); for (int r = 0; r < ping.nRanges(); r++) { @@ -49,8 +49,8 @@ std::vector HistogramGenerator::GenerateUint16( return result; } -std::vector HistogramGenerator::GenerateUint32( - const AbstractSonarInterface &ping) { +std::vector +HistogramGenerator::GenerateUint32(const AbstractSonarInterface &ping) { std::vector result(256, 0); const float logMax = log10(UINT32_MAX); @@ -59,7 +59,8 @@ std::vector HistogramGenerator::GenerateUint32( for (int b = 0; b < ping.nBearings(); b++) { const auto val = ping.intensity_uint32(AzimuthRangeIndices(b, r)); - if (val == 0) continue; + if (val == 0) + continue; const float l = log10(val); @@ -68,7 +69,8 @@ std::vector HistogramGenerator::GenerateUint32( // std::cerr << "val = " << val << "; l = " << l << "; idx = " << idx << // std::endl; - if ((idx < 0) || (idx > result.size())) continue; + if ((idx < 0) || (idx > result.size())) + continue; result[idx]++; } } @@ -76,4 +78,4 @@ std::vector HistogramGenerator::GenerateUint32( return result; } -} // namespace sonar_image_proc +} // namespace libdrawsonar diff --git a/lib/OldDrawSonar.cpp b/lib/OldDrawSonar.cpp index 4cc018b..39e7f5b 100644 --- a/lib/OldDrawSonar.cpp +++ b/lib/OldDrawSonar.cpp @@ -6,13 +6,13 @@ #include #include -#include "sonar_image_proc/DrawSonar.h" +#include "libdrawsonar/DrawSonar.h" #ifndef THETA_SHIFT #define THETA_SHIFT PI; #endif -namespace sonar_image_proc { +namespace libdrawsonar { namespace old_api { @@ -45,8 +45,10 @@ cv::Size calculateImageSize(const AbstractSonarInterface &ping, cv::Size hint, } // Ensure w and h are both divisible by zero - if (w % 2) w++; - if (h % 2) h++; + if (w % 2) + w++; + if (h % 2) + h++; return Size(w, h); } @@ -112,7 +114,8 @@ cv::Mat drawSonar(const AbstractSonarInterface &ping, const Mat &mat, } for (int r = 0; r < nRanges; ++r) { - if (ping.range(r) > rangeMax) continue; + if (ping.range(r) > rangeMax) + continue; for (int b = 0; b < nBeams; ++b) { const float range = ping.range(r); @@ -135,5 +138,5 @@ cv::Mat drawSonar(const AbstractSonarInterface &ping, const Mat &mat, return out; } -} // namespace old_api -} // namespace sonar_image_proc +} // namespace old_api +} // namespace libdrawsonar diff --git a/lib/SonarDrawer.cpp b/lib/SonarDrawer.cpp index 392bfc5..db92e93 100644 --- a/lib/SonarDrawer.cpp +++ b/lib/SonarDrawer.cpp @@ -5,12 +5,12 @@ #include #include -#include "sonar_image_proc/DrawSonar.h" -#include "sonar_image_proc/OverlayImage.h" +#include "libdrawsonar/DrawSonar.h" +#include "libdrawsonar/OverlayImage.h" -namespace sonar_image_proc { +namespace libdrawsonar { -using sonar_image_proc::AbstractSonarInterface; +using libdrawsonar::AbstractSonarInterface; static float deg2radf(float deg) { return deg * M_PI / 180.0; } static float rad2degf(float rad) { return rad * 180.0 / M_PI; } @@ -97,10 +97,11 @@ bool SonarDrawer::Cached::isValid(const AbstractSonarInterface &ping) const { // ==== SonarDrawer::CachedMap ==== -SonarDrawer::CachedMap::MapPair SonarDrawer::CachedMap::operator()( - const AbstractSonarInterface &ping) { +SonarDrawer::CachedMap::MapPair +SonarDrawer::CachedMap::operator()(const AbstractSonarInterface &ping) { // _scMap[12] are mutable to break out of const - if (!isValid(ping)) create(ping); + if (!isValid(ping)) + create(ping); return std::make_pair(_scMap1, _scMap2); } @@ -121,7 +122,8 @@ void SonarDrawer::CachedMap::create(const AbstractSonarInterface &ping) { const int originx = abs(minusWidth); const cv::Size imgSize(width, nRanges); - if ((width <= 0) || (nRanges <= 0)) return; + if ((width <= 0) || (nRanges <= 0)) + return; newmap.create(imgSize, CV_32FC2); @@ -171,7 +173,8 @@ void SonarDrawer::CachedMap::create(const AbstractSonarInterface &ping) { } bool SonarDrawer::CachedMap::isValid(const AbstractSonarInterface &ping) const { - if (_scMap1.empty() || _scMap2.empty()) return false; + if (_scMap1.empty() || _scMap2.empty()) + return false; return Cached::isValid(ping); } @@ -181,17 +184,21 @@ bool SonarDrawer::CachedMap::isValid(const AbstractSonarInterface &ping) const { bool SonarDrawer::CachedOverlay::isValid(const AbstractSonarInterface &ping, const cv::Mat &sonarImage, const OverlayConfig &config) const { - if (sonarImage.size() != _overlay.size()) return false; + if (sonarImage.size() != _overlay.size()) + return false; - if (_config_used != config) return false; + if (_config_used != config) + return false; return Cached::isValid(ping); } -const cv::Mat &SonarDrawer::CachedOverlay::operator()( - const AbstractSonarInterface &ping, const cv::Mat &sonarImage, - const OverlayConfig &config) { - if (!isValid(ping, sonarImage, config)) create(ping, sonarImage, config); +const cv::Mat & +SonarDrawer::CachedOverlay::operator()(const AbstractSonarInterface &ping, + const cv::Mat &sonarImage, + const OverlayConfig &config) { + if (!isValid(ping, sonarImage, config)) + create(ping, sonarImage, config); return _overlay; } @@ -318,4 +325,4 @@ void SonarDrawer::CachedOverlay::create(const AbstractSonarInterface &ping, _config_used = config; } -} // namespace sonar_image_proc +} // namespace libdrawsonar diff --git a/libdrawsonar.repos b/libdrawsonar.repos new file mode 100644 index 0000000..90261ee --- /dev/null +++ b/libdrawsonar.repos @@ -0,0 +1,5 @@ +repositories: + marine_msgs: + type: git + url: https://github.com/apl-ocean-engineering/marine_msgs + version: ros2 diff --git a/package.xml b/package.xml index e63c7c7..a64647e 100644 --- a/package.xml +++ b/package.xml @@ -1,36 +1,20 @@ - - sonar_image_proc + + libdrawsonar 0.0.1 Common code for manipulating imaging sonar data. + Aaron Marburg Aaron Marburg - - - BSD - catkin - - roscpp - rospy - - marine_acoustic_msgs - cv_bridge - image_transport - nodelet_topic_tools - std_msgs - - dynamic_reconfigure - nodelet - rosbag_storage - - dynamic_reconfigure - nodelet + catkin + ament_cmake - + catkin + ament_cmake diff --git a/python/draw_sonar/__init__.py b/python/draw_sonar/__init__.py index 705f190..b71854b 100644 --- a/python/draw_sonar/__init__.py +++ b/python/draw_sonar/__init__.py @@ -1,6 +1,6 @@ +from py_draw_sonar import * # NOQA -from py_draw_sonar import * ## A trivial test function to check if the module is loading properly -def pyadd(a,b): - return a+b +def pyadd(a, b): + return a + b diff --git a/python/histogram_drawer b/python/histogram_drawer index 93a32a1..53b6c95 100755 --- a/python/histogram_drawer +++ b/python/histogram_drawer @@ -9,49 +9,49 @@ from cv_bridge import CvBridge import io import numpy as np import matplotlib -matplotlib.use('agg') # turn off interactive backend + +matplotlib.use("agg") # turn off interactive backend import matplotlib.pyplot as plt -class HistogramDrawer: +class HistogramDrawer: def __init__(self): - self.pub = rospy.Publisher("drawn_histogram", Image, queue_size=10) self.sub = rospy.Subscriber("histogram", UInt32MultiArray, self.rx_histogram) self.bin_max = 0 def rx_histogram(self, data): - #print(data.data) + # print(data.data) xmax = len(data.data) ## Regardless, compress to 128 bins .. this algorithm assumes the new # number of bins is a factor of the original histogram bins - d = np.reshape(data.data, (128,-1), 'C') + d = np.reshape(data.data, (128, -1), "C") - new_histogram = np.sum(d,axis=1) + new_histogram = np.sum(d, axis=1) self.bin_max = np.max((self.bin_max, max(new_histogram))) - #print(new_histogram) + # print(new_histogram) fig, ax = plt.subplots() plt.yscale("log") - ax.plot(np.linspace(0,xmax,128), new_histogram,'b.-') + ax.plot(np.linspace(0, xmax, 128), new_histogram, "b.-") ax.set_xlabel("Intensity") ax.set_ylabel("Count") - ax.set_ylim([1, self.bin_max*2]) + ax.set_ylim([1, self.bin_max * 2]) with io.BytesIO() as buff: - fig.savefig(buff, format='raw') + fig.savefig(buff, format="raw") buff.seek(0) data = np.frombuffer(buff.getvalue(), dtype=np.uint8) w, h = fig.canvas.get_width_height() im = data.reshape((int(h), int(w), -1)) # Drop alpha channel - im = im[:,:,:3] + im = im[:, :, :3] bridge = CvBridge() image_message = bridge.cv2_to_imgmsg(im, encoding="rgb8") @@ -61,11 +61,9 @@ class HistogramDrawer: plt.close(fig) - if __name__ == "__main__": + rospy.init_node("histogram_drawer", anonymous=True) - rospy.init_node('histogram_drawer', anonymous=True) - - drawer = HistogramDrawer() + drawer = HistogramDrawer() - rospy.spin(); + rospy.spin() diff --git a/python/ndarray_converter.cpp b/python/ndarray_converter.cpp index 158999c..8a38376 100644 --- a/python/ndarray_converter.cpp +++ b/python/ndarray_converter.cpp @@ -45,36 +45,36 @@ static int failmsg(const char *fmt, ...) { } class PyAllowThreads { - public: +public: PyAllowThreads() : _state(PyEval_SaveThread()) {} ~PyAllowThreads() { PyEval_RestoreThread(_state); } - private: +private: PyThreadState *_state; }; class PyEnsureGIL { - public: +public: PyEnsureGIL() : _state(PyGILState_Ensure()) {} ~PyEnsureGIL() { PyGILState_Release(_state); } - private: +private: PyGILState_STATE _state; }; -#define ERRWRAP2(expr) \ - try { \ - PyAllowThreads allowThreads; \ - expr; \ - } catch (const cv::Exception &e) { \ - PyErr_SetString(opencv_error, e.what()); \ - return 0; \ +#define ERRWRAP2(expr) \ + try { \ + PyAllowThreads allowThreads; \ + expr; \ + } catch (const cv::Exception &e) { \ + PyErr_SetString(opencv_error, e.what()); \ + return 0; \ } using namespace cv; class NumpyAllocator : public MatAllocator { - public: +public: NumpyAllocator() { stdAllocator = Mat::getStdAllocator(); } ~NumpyAllocator() {} @@ -83,7 +83,8 @@ class NumpyAllocator : public MatAllocator { UMatData *u = new UMatData(this); u->data = u->origdata = (uchar *)PyArray_DATA((PyArrayObject *)o); npy_intp *_strides = PyArray_STRIDES((PyArrayObject *)o); - for (int i = 0; i < dims - 1; i++) step[i] = (size_t)_strides[i]; + for (int i = 0; i < dims - 1; i++) + step[i] = (size_t)_strides[i]; step[dims - 1] = CV_ELEM_SIZE(type); u->size = sizes[0] * step[0]; u->userdata = o; @@ -120,8 +121,10 @@ class NumpyAllocator : public MatAllocator { : f * NPY_ULONGLONG + (f ^ 1) * NPY_UINT; int i, dims = dims0; cv::AutoBuffer _sizes(dims + 1); - for (i = 0; i < dims; i++) _sizes[i] = sizes[i]; - if (cn > 1) _sizes[dims++] = cn; + for (i = 0; i < dims; i++) + _sizes[i] = sizes[i]; + if (cn > 1) + _sizes[dims++] = cn; PyObject *o = PyArray_SimpleNew(dims, _sizes, typenum); if (!o) CV_Error_(Error::StsError, @@ -141,7 +144,8 @@ class NumpyAllocator : public MatAllocator { } void deallocate(UMatData *u) const { - if (!u) return; + if (!u) + return; PyEnsureGIL gil; CV_Assert(u->urefcount >= 0); CV_Assert(u->refcount >= 0); @@ -160,7 +164,8 @@ NumpyAllocator g_numpyAllocator; bool NDArrayConverter::toMat(PyObject *o, Mat &m) { bool allowND = true; if (!o || o == Py_None) { - if (!m.data) m.allocator = &g_numpyAllocator; + if (!m.data) + m.allocator = &g_numpyAllocator; return true; } @@ -317,7 +322,8 @@ bool NDArrayConverter::toMat(PyObject *o, Mat &m) { } PyObject *NDArrayConverter::toNDArray(const cv::Mat &m) { - if (!m.data) Py_RETURN_NONE; + if (!m.data) + Py_RETURN_NONE; Mat temp, *p = (Mat *)&m; if (!p->u || p->allocator != &g_numpyAllocator) { temp.allocator = &g_numpyAllocator; diff --git a/python/ndarray_converter.h b/python/ndarray_converter.h index 3b87f94..81ba002 100644 --- a/python/ndarray_converter.h +++ b/python/ndarray_converter.h @@ -6,7 +6,7 @@ #include class NDArrayConverter { - public: +public: // must call this first, or the other routines don't work! static bool init_numpy(); @@ -23,9 +23,8 @@ class NDArrayConverter { namespace pybind11 { namespace detail { -template <> -struct type_caster { - public: +template <> struct type_caster { +public: PYBIND11_TYPE_CASTER(cv::Mat, _("numpy.ndarray")); bool load(handle src, bool) { @@ -37,7 +36,7 @@ struct type_caster { } }; -} // namespace detail -} // namespace pybind11 +} // namespace detail +} // namespace pybind11 #endif diff --git a/python/old_setup.py b/python/old_setup.py index 7289d88..9a610a7 100644 --- a/python/old_setup.py +++ b/python/old_setup.py @@ -2,12 +2,8 @@ # Much of this copied from https://github.com/edmBernard/pybind11_opencv_numpy # -import fnmatch -import os -from os.path import dirname, exists, join from setuptools import find_packages, setup, Extension from setuptools.command.build_ext import build_ext -import subprocess import sys import setuptools @@ -15,20 +11,23 @@ # pybind-specific compilation stuff # + class get_pybind_include(object): """Helper class to determine the pybind11 include path The purpose of this class is to postpone importing pybind11 until it is actually installed, so that the ``get_include()`` - method can be invoked. """ + method can be invoked.""" def __init__(self, user=False): self.user = user def __str__(self): import pybind11 + return pybind11.get_include(self.user) + # As of Python 3.6, CCompiler has a `has_flag` method. # cf http://bugs.python.org/issue26689 def has_flag(compiler, flagname): @@ -36,8 +35,9 @@ def has_flag(compiler, flagname): the specified compiler. """ import tempfile - with tempfile.NamedTemporaryFile('w', suffix='.cpp') as f: - f.write('int main (int argc, char **argv) { return 0; }') + + with tempfile.NamedTemporaryFile("w", suffix=".cpp") as f: + f.write("int main (int argc, char **argv) { return 0; }") try: compiler.compile([f.name], extra_postargs=[flagname]) except setuptools.distutils.errors.CompileError: @@ -50,24 +50,24 @@ def cpp_flag(compiler): The c++14 is preferred over c++11 (when it is available). """ - if has_flag(compiler, '-std=c++14'): - return '-std=c++14' - elif has_flag(compiler, '-std=c++11'): - return '-std=c++11' + if has_flag(compiler, "-std=c++14"): + return "-std=c++14" + elif has_flag(compiler, "-std=c++11"): + return "-std=c++11" else: - raise RuntimeError('Unsupported compiler -- at least C++11 support ' - 'is needed!') + raise RuntimeError("Unsupported compiler -- at least C++11 support is needed!") class BuildExt(build_ext): """A custom build extension for adding compiler-specific options.""" + c_opts = { - 'msvc': ['/EHsc'], - 'unix': [], + "msvc": ["/EHsc"], + "unix": [], } - if sys.platform == 'darwin': - c_opts['unix'] += ['-stdlib=libc++', '-mmacosx-version-min=10.7'] + if sys.platform == "darwin": + c_opts["unix"] += ["-stdlib=libc++", "-mmacosx-version-min=10.7"] def build_extensions(self): ct = self.compiler.compiler_type @@ -76,45 +76,46 @@ def build_extensions(self): for arg in ext_modules[0].extra_compile_args: opts.append(arg) - if ct == 'unix': - opts.append('-s') # strip - opts.append('-g0') # remove debug symbols + if ct == "unix": + opts.append("-s") # strip + opts.append("-g0") # remove debug symbols opts.append(cpp_flag(self.compiler)) - if has_flag(self.compiler, '-fvisibility=hidden'): - opts.append('-fvisibility=hidden') + if has_flag(self.compiler, "-fvisibility=hidden"): + opts.append("-fvisibility=hidden") for ext in self.extensions: ext.extra_compile_args = opts build_ext.build_extensions(self) + ext_modules = [ Extension( - 'serdp_common', + "serdp_common", [ - 'serdp_common_py.cpp', - 'ndarray_converter.cpp', - '../lib/DrawSonar.cpp', - '../lib/DataStructures.cpp' + "serdp_common_py.cpp", + "ndarray_converter.cpp", + "../lib/DrawSonar.cpp", + "../lib/DataStructures.cpp", ], include_dirs=[ # Path to pybind11 headers get_pybind_include(), get_pybind_include(user=True), - "../include/" + "../include/", ], - extra_compile_args=['-DABSTRACT_SONAR_INTERFACE_ONLY'], - libraries=['opencv_core', 'opencv_highgui'], - language='c++', + extra_compile_args=["-DABSTRACT_SONAR_INTERFACE_ONLY"], + libraries=["opencv_core", "opencv_highgui"], + language="c++", ), ] setup( - name='serdp_common', - version='0.1', - author='Aaron Marburg', - author_email='amarburg@uw.edu', + name="serdp_common", + version="0.1", + author="Aaron Marburg", + author_email="amarburg@uw.edu", packages=find_packages(), ext_modules=ext_modules, install_requires=None, - cmdclass={'build_ext': BuildExt}, + cmdclass={"build_ext": BuildExt}, zip_safe=False, ) diff --git a/python/py_draw_sonar.cpp b/python/py_draw_sonar.cpp index 7024a5e..5907a2f 100644 --- a/python/py_draw_sonar.cpp +++ b/python/py_draw_sonar.cpp @@ -63,12 +63,12 @@ cv::Mat cloneimg(cv::Mat image) { return image.clone(); } int add(int i, int j) { return i + j; } class AddClass { - public: +public: AddClass(int value) : value(value) {} cv::Mat add(cv::Mat input) { return input + this->value; } - private: +private: int value; }; diff --git a/python/sonar_image_proc/sonar_msg_metadata.py b/python/sonar_image_proc/sonar_msg_metadata.py index fd400d4..4ddc1f2 100644 --- a/python/sonar_image_proc/sonar_msg_metadata.py +++ b/python/sonar_image_proc/sonar_msg_metadata.py @@ -1,4 +1,3 @@ -#! /usr/bin/env python3 """ Copyright 2023 University of Washington Applied Physics Laboratory Author: Marc Micatka & Laura Lindzey diff --git a/python/test.py b/python/test.py index ef044ea..778f4f4 100644 --- a/python/test.py +++ b/python/test.py @@ -1,4 +1,4 @@ import draw_sonar -print( draw_sonar.pyadd(1,2) ) -print( draw_sonar.cppadd(1,2) ) +print(draw_sonar.pyadd(1, 2)) +print(draw_sonar.cppadd(1, 2)) diff --git a/ros/cfg/DrawSonar.cfg b/ros/cfg/DrawSonar.cfg deleted file mode 100644 index ea7fe22..0000000 --- a/ros/cfg/DrawSonar.cfg +++ /dev/null @@ -1,18 +0,0 @@ -#!/usr/bin/env python -PACKAGE = "sonar_image_proc" - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -gen.add("line_alpha", double_t, 0, "Alpha for lines", 0.5, 0, 1.0) -gen.add("line_thickness", int_t, 0, "Line thickness", 1, 1, 5) - -gen.add("range_spacing", double_t, 0, "Spacing of range marks (0 for auto)", 0, 0, 100) -gen.add("bearing_spacing", double_t, 0, "Spacing of bearing radials", 20, 0, 100) - -gen.add("log_scale", bool_t, 0, "Use log scale for intensity", False) -gen.add("min_db", double_t, 0, "Minimum intensity in log scale (in db, 0 for full range)", 0, -221, 0) -gen.add("max_db", double_t, 0, "Maximum intensity in log scale (in db, 0 for full range)", 0, -221, 0) - -exit(gen.generate(PACKAGE, "sonar_image_proc", "DrawSonar")) diff --git a/ros/include/sonar_image_proc/sonar_image_msg_interface.h b/ros/include/sonar_image_proc/sonar_image_msg_interface.h deleted file mode 100644 index 2b8b04e..0000000 --- a/ros/include/sonar_image_proc/sonar_image_msg_interface.h +++ /dev/null @@ -1,237 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// -// An implementation of an AbstractSonarInterface which -// wraps a ROS marine_acoustic_msgs::SonarImage -// - -#pragma once - -#include - -#include "sonar_image_proc/AbstractSonarInterface.h" - -namespace sonar_image_proc { - -using marine_acoustic_msgs::ProjectedSonarImage; -using sonar_image_proc::AbstractSonarInterface; -using std::vector; - -struct SonarImageMsgInterface - : public sonar_image_proc::AbstractSonarInterface { - explicit SonarImageMsgInterface( - const marine_acoustic_msgs::ProjectedSonarImage::ConstPtr &ping) - : _ping(ping), do_log_scale_(false) { - // Vertical field of view is determined by comparing - // z / sqrt(x^2 + y^2) to tan(elevation_beamwidth/2) - _verticalTanSquared = - // NOTE(lindzey): The old message assumed a constant elevation - // beamwidth; - // the multibeam people insisted that it be per-beam. For now, this - // assumes that they're all the same. - // TODO(lindzey): Look into whether averaging would be better, or if we - // should create an array of verticalTanSquared. - // TODO(lindzey): Handle empty-array case. - std::pow(std::tan(ping->ping_info.tx_beamwidths[0] / 2.0), 2); - - for (const auto pt : ping->beam_directions) { - auto az = atan2(-1 * pt.y, pt.z); - _ping_azimuths.push_back(az); - } - } - - // n.b. do_log_scale is a mode switch which causes the intensity_* - // functions in this SonarImageMsgInterface use a log (db) scale - // for 32-bit data **ONLY** -- functionality is unchanged for 8- - // and 16-bit data - // - // It maps a given 32-bit intensity value i to a fraction of the - // full scale of a 32-bit uint in the interval [0.0, 1.0] - // It then takes the db value of that: - // - // l = 10 * log( i / (2^32-1) ) - // - // Such that l is between - // ~ -96 for i = 1 - // and - // 0.0 for i = UINT32_MAX - // - // The intensity_* functions create a linear map m() from [min_db, max_db] - // to the full range of the returned type (e.g. uint8, uint16, etc), - // and returns m(l) - // - // If min_db is 0.0, then the full range value - // min_db = (10*log(1/(2^32-1))) is used - // If max_db is 0.0, then the full range value - // max_db = 0.0 is used - // - // So for example if i = 2^31 (half the full range of a uint32), l = -3 db - // This is approx 0.97 of the full scale in logspace - // - // With min_db = 0, max_db = 0 (full range) - // intensity_uint8(i) will return approx 247 (0.97 * 255) - // - // With min_db = -10, max_db = 0 - // intensity_uint8(i) will return approx 178 (0.7 * 255) - // - // With min_db = -1, max_db = 0 - // l(i) is less than min_db and intensity_uint8(i) will return 0 - // - // With min_db = 0, max_db = -10 - // l(i) is greater than max_db and intensity_uint8(i) will return 255 - // - void do_log_scale(float min_db, float max_db) { - do_log_scale_ = true; - min_db_ = min_db; - max_db_ = max_db; - range_db_ = max_db - min_db; - } - - AbstractSonarInterface::DataType_t data_type() const override { - if (_ping->image.dtype == _ping->image.DTYPE_UINT8) - return AbstractSonarInterface::TYPE_UINT8; - else if (_ping->image.dtype == _ping->image.DTYPE_UINT16) - return AbstractSonarInterface::TYPE_UINT16; - else if (_ping->image.dtype == _ping->image.DTYPE_UINT32) - return AbstractSonarInterface::TYPE_UINT32; - - // - return AbstractSonarInterface::TYPE_NONE; - } - - const std::vector &ranges() const override { return _ping->ranges; } - - const std::vector &azimuths() const override { return _ping_azimuths; } - - float verticalTanSquared() const { return _verticalTanSquared; } - - // When the underlying data is 8-bit, returns that exact value - // from the underlying data - // - // If the underlying data is 16-bit, it returns a scaled value. - uint8_t intensity_uint8(const AzimuthRangeIndices &idx) const override { - if (do_log_scale_ && (_ping->image.dtype == _ping->image.DTYPE_UINT32)) { - return intensity_float_log(idx) * UINT8_MAX; - } - - if (_ping->image.dtype == _ping->image.DTYPE_UINT8) { - return read_uint8(idx); - } else if (_ping->image.dtype == _ping->image.DTYPE_UINT16) { - return read_uint16(idx) >> 8; - } else if (_ping->image.dtype == _ping->image.DTYPE_UINT32) { - return read_uint32(idx) >> 24; - } - - return 0; - } - - uint16_t intensity_uint16(const AzimuthRangeIndices &idx) const override { - // Truncate 32bit intensities - if (do_log_scale_ && (_ping->image.dtype == _ping->image.DTYPE_UINT32)) { - return intensity_float_log(idx) * UINT16_MAX; - } - - if (_ping->image.dtype == _ping->image.DTYPE_UINT8) { - return read_uint8(idx) << 8; - } else if (_ping->image.dtype == _ping->image.DTYPE_UINT16) { - return read_uint16(idx); - } else if (_ping->image.dtype == _ping->image.DTYPE_UINT32) { - return read_uint32(idx) >> 16; - } - return 0; - } - - uint32_t intensity_uint32(const AzimuthRangeIndices &idx) const override { - if (do_log_scale_ && (_ping->image.dtype == _ping->image.DTYPE_UINT32)) { - return intensity_float_log(idx) * UINT32_MAX; - } - - if (_ping->image.dtype == _ping->image.DTYPE_UINT8) { - return read_uint8(idx) << 24; - } else if (_ping->image.dtype == _ping->image.DTYPE_UINT16) { - return read_uint16(idx) << 16; - } else if (_ping->image.dtype == _ping->image.DTYPE_UINT32) { - return read_uint32(idx); - } - return 0; - } - - float intensity_float(const AzimuthRangeIndices &idx) const override { - if (do_log_scale_ && (_ping->image.dtype == _ping->image.DTYPE_UINT32)) { - return intensity_float_log(idx); - } - - if (_ping->image.dtype == _ping->image.DTYPE_UINT8) { - return static_cast(read_uint8(idx)) / UINT8_MAX; - } else if (_ping->image.dtype == _ping->image.DTYPE_UINT16) { - return static_cast(read_uint16(idx)) / UINT16_MAX; - } else if (_ping->image.dtype == _ping->image.DTYPE_UINT32) { - return static_cast(read_uint32(idx)) / UINT32_MAX; - } - return 0.0; - } - - protected: - marine_acoustic_msgs::ProjectedSonarImage::ConstPtr _ping; - - float _verticalTanSquared; - std::vector _ping_azimuths; - - size_t index(const AzimuthRangeIndices &idx) const { - int data_size; - if (_ping->image.dtype == _ping->image.DTYPE_UINT8) { - data_size = 1; - } else if (_ping->image.dtype == _ping->image.DTYPE_UINT16) { - data_size = 2; - } else if (_ping->image.dtype == _ping->image.DTYPE_UINT32) { - data_size = 4; - } else { - assert(false); - } - - return data_size * ((idx.range() * nBearings()) + idx.azimuth()); - } - - // "raw" read functions. Assumes the data type has already been checked - uint32_t read_uint8(const AzimuthRangeIndices &idx) const { - assert(_ping->image.dtype == _ping->image.DTYPE_UINT8); - const auto i = index(idx); - - return (_ping->image.data[i]); - } - - uint32_t read_uint16(const AzimuthRangeIndices &idx) const { - assert(_ping->image.dtype == _ping->image.DTYPE_UINT16); - const auto i = index(idx); - - return (static_cast(_ping->image.data[i]) | - (static_cast(_ping->image.data[i + 1]) << 8)); - } - - uint32_t read_uint32(const AzimuthRangeIndices &idx) const { - assert(_ping->image.dtype == _ping->image.DTYPE_UINT32); - const auto i = index(idx); - - const uint32_t v = - (static_cast(_ping->image.data[i]) | - (static_cast(_ping->image.data[i + 1]) << 8) | - (static_cast(_ping->image.data[i + 2]) << 16) | - (static_cast(_ping->image.data[i + 3]) << 24)); - return v; - } - - float intensity_float_log(const AzimuthRangeIndices &idx) const { - const auto intensity = read_uint32(idx); - const auto v = - log(static_cast(std::max((uint)1, intensity)) / UINT32_MAX) * - 10; // dbm - - const auto min_db = (min_db_ == 0 ? log(1.0 / UINT32_MAX) * 10 : min_db_); - - return std::min(1.0, std::max(0.0, (v - min_db) / range_db_)); - } - - bool do_log_scale_; - float min_db_, max_db_, range_db_; -}; - -} // namespace sonar_image_proc diff --git a/ros/src/draw_sonar_node.cpp b/ros/src/draw_sonar_node.cpp deleted file mode 100644 index b8ece56..0000000 --- a/ros/src/draw_sonar_node.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// - -#include "nodelet/loader.h" -#include "ros/ros.h" - -int main(int argc, char **argv) { - ros::init(argc, argv, "draw_sonar"); - - nodelet::Loader nodelet; - nodelet::M_string remap(ros::names::getRemappings()); - nodelet::V_string nargv; - - nodelet.load(ros::this_node::getName(), "sonar_image_proc/draw_sonar", remap, - nargv); - - ros::spin(); - return 0; -} diff --git a/ros/src/draw_sonar_nodelet.cpp b/ros/src/draw_sonar_nodelet.cpp deleted file mode 100644 index b402a00..0000000 --- a/ros/src/draw_sonar_nodelet.cpp +++ /dev/null @@ -1,240 +0,0 @@ -// Copyright 2021-2022 University of Washington Applied Physics Laboratory -// Author: Aaron Marburg - -#include -#include -#include -#include - -#include "marine_acoustic_msgs/ProjectedSonarImage.h" -#include "nodelet/nodelet.h" -#include "ros/ros.h" - -// For uploading drawn sonar images to Image topic -#include -#include -#include -#include -#include -#include - -#include "sonar_image_proc/ColorMaps.h" -#include "sonar_image_proc/DrawSonar.h" -#include "sonar_image_proc/HistogramGenerator.h" -#include "sonar_image_proc/SonarDrawer.h" -#include "sonar_image_proc/sonar_image_msg_interface.h" - -// Subscribes to sonar message topic, draws using opencv then publishes -// result - -namespace draw_sonar { - -using namespace std; -using namespace cv; - -using sonar_image_proc::SonarImageMsgInterface; -using std_msgs::UInt32MultiArray; - -using sonar_image_proc::HistogramGenerator; - -using sonar_image_proc::InfernoColorMap; -using sonar_image_proc::InfernoSaturationColorMap; -using sonar_image_proc::SonarColorMap; - -class DrawSonarNodelet : public nodelet::Nodelet { - public: - DrawSonarNodelet() - : Nodelet(), - _maxRange(0.0), - _colorMap(new InfernoColorMap) // Set a reasonable default - { - ; - } - - virtual ~DrawSonarNodelet() { ; } - - private: - virtual void onInit() { - ros::NodeHandle nh = getMTNodeHandle(); - ros::NodeHandle pnh = getMTPrivateNodeHandle(); - - pnh.param("max_range", _maxRange, 0.0); - - pnh.param("publish_old", _publishOldApi, false); - pnh.param("publish_timing", _publishTiming, true); - - pnh.param("publish_histogram", _publishHistogram, false); - - std::string colorMapName; - pnh.param("color_map", colorMapName, "inferno"); - - // Set color map - setColorMap(colorMapName); - - if (_maxRange > 0.0) { - NODELET_INFO_STREAM("Only drawing to max range " << _maxRange); - } - - subSonarImage_ = nh.subscribe( - "sonar_image", 10, &DrawSonarNodelet::sonarImageCallback, this); - - pub_ = nh.advertise("drawn_sonar", 10); - osdPub_ = nh.advertise("drawn_sonar_osd", 10); - rectPub_ = nh.advertise("drawn_sonar_rect", 10); - - if (_publishOldApi) - oldPub_ = nh.advertise("old_drawn_sonar", 10); - - if (_publishTiming) - timingPub_ = - nh.advertise("sonar_image_proc_timing", 10); - - if (_publishHistogram) - histogramPub_ = nh.advertise("histogram", 10); - - dyn_cfg_server_.reset(new DynamicReconfigureServer(pnh)); - dyn_cfg_server_->setCallback(std::bind(&DrawSonarNodelet::dynCfgCallback, - this, std::placeholders::_1, - std::placeholders::_2)); - - ROS_DEBUG("draw_sonar ready to run..."); - } - - void cvBridgeAndPublish( - const marine_acoustic_msgs::ProjectedSonarImage::ConstPtr &msg, - const cv::Mat &mat, ros::Publisher &pub) { - cv_bridge::CvImage img_bridge(msg->header, - sensor_msgs::image_encodings::RGB8, mat); - - sensor_msgs::Image output_msg; - img_bridge.toImageMsg(output_msg); - pub.publish(output_msg); - } - - void sonarImageCallback( - const marine_acoustic_msgs::ProjectedSonarImage::ConstPtr &msg) { - ROS_FATAL_COND(!_colorMap, "Colormap is undefined, this shouldn't happen"); - - SonarImageMsgInterface interface(msg); - if (log_scale_) { - interface.do_log_scale(min_db_, max_db_); - } - - ros::WallDuration oldApiElapsed, rectElapsed, mapElapsed, histogramElapsed; - - if (_publishOldApi) { - ros::WallTime begin = ros::WallTime::now(); - - // Used to be a configurable parameter, but now only meaningful - // in the deprecated API - const int pixPerRangeBin = 2; - - cv::Size sz = sonar_image_proc::old_api::calculateImageSize( - interface, cv::Size(0, 0), pixPerRangeBin, _maxRange); - cv::Mat mat(sz, CV_8UC3); - mat = sonar_image_proc::old_api::drawSonar(interface, mat, *_colorMap, - _maxRange); - - cvBridgeAndPublish(msg, mat, oldPub_); - - oldApiElapsed = ros::WallTime::now() - begin; - } - - if (_publishHistogram) { - ros::WallTime begin = ros::WallTime::now(); - - auto histogramOut = UInt32MultiArray(); - histogramOut.data = HistogramGenerator::Generate(interface); - - histogramPub_.publish(histogramOut); - - histogramElapsed = ros::WallTime::now() - begin; - } - - { - ros::WallTime begin = ros::WallTime::now(); - - cv::Mat rectMat = _sonarDrawer.drawRectSonarImage(interface, *_colorMap); - - // Rotate rectangular image to the more expected format where zero range - // is at the bottom of the image, with negative azimuth to the right - // aka (rotated 90 degrees CCW) - cv::Mat rotatedRect; - cv::rotate(rectMat, rotatedRect, cv::ROTATE_90_COUNTERCLOCKWISE); - cvBridgeAndPublish(msg, rotatedRect, rectPub_); - - rectElapsed = ros::WallTime::now() - begin; - begin = ros::WallTime::now(); - - cv::Mat sonarMat = _sonarDrawer.remapRectSonarImage(interface, rectMat); - cvBridgeAndPublish(msg, sonarMat, pub_); - - if (osdPub_.getNumSubscribers() > 0) { - cv::Mat osdMat = _sonarDrawer.drawOverlay(interface, sonarMat); - cvBridgeAndPublish(msg, osdMat, osdPub_); - } - - mapElapsed = ros::WallTime::now() - begin; - } - - if (_publishTiming) { - ostringstream output; - - output << "{"; - output << "\"draw_total\" : " << (mapElapsed + rectElapsed).toSec(); - output << ", \"rect\" : " << rectElapsed.toSec(); - output << ", \"map\" : " << mapElapsed.toSec(); - - if (_publishOldApi) output << ", \"old_api\" : " << oldApiElapsed.toSec(); - if (_publishHistogram) - output << ", \"histogram\" : " << histogramElapsed.toSec(); - - output << "}"; - - std_msgs::String out_msg; - out_msg.data = output.str(); - - timingPub_.publish(out_msg); - } - } - - void dynCfgCallback(sonar_image_proc::DrawSonarConfig &config, - uint32_t level) { - _sonarDrawer.overlayConfig() - .setRangeSpacing(config.range_spacing) - .setRadialSpacing(config.bearing_spacing) - .setLineAlpha(config.line_alpha) - .setLineThickness(config.line_thickness); - - log_scale_ = config.log_scale; - min_db_ = config.min_db; - max_db_ = config.max_db; - } - - void setColorMap(const std::string &colorMapName) { - // TBD actually implement the parameter processing here... - _colorMap.reset(new InfernoSaturationColorMap()); - } - - ros::Subscriber subSonarImage_; - ros::Publisher pub_, rectPub_, osdPub_, oldPub_, timingPub_, histogramPub_; - - typedef dynamic_reconfigure::Server - DynamicReconfigureServer; - std::shared_ptr dyn_cfg_server_; - - sonar_image_proc::SonarDrawer _sonarDrawer; - - float _maxRange; - bool _publishOldApi, _publishTiming, _publishHistogram; - - float min_db_, max_db_; - bool log_scale_; - - std::unique_ptr _colorMap; -}; - -} // namespace draw_sonar - -#include -PLUGINLIB_EXPORT_CLASS(draw_sonar::DrawSonarNodelet, nodelet::Nodelet); diff --git a/ros/src/sonar_postprocessor_node.cpp b/ros/src/sonar_postprocessor_node.cpp deleted file mode 100644 index 91fa87f..0000000 --- a/ros/src/sonar_postprocessor_node.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// Copyright 2021 University of Washington Applied Physics Laboratory -// - -#include "nodelet/loader.h" -#include "ros/ros.h" - -int main(int argc, char **argv) { - ros::init(argc, argv, "sonar_postprocessor"); - - nodelet::Loader nodelet; - nodelet::M_string remap(ros::names::getRemappings()); - nodelet::V_string nargv; - - nodelet.load(ros::this_node::getName(), - "sonar_image_proc/sonar_postprocessor", remap, nargv); - - ros::spin(); - return 0; -} diff --git a/ros/src/sonar_postprocessor_nodelet.cpp b/ros/src/sonar_postprocessor_nodelet.cpp deleted file mode 100644 index 04bc625..0000000 --- a/ros/src/sonar_postprocessor_nodelet.cpp +++ /dev/null @@ -1,114 +0,0 @@ -// Copyright 2021-2022 University of Washington Applied Physics Laboratory -// Author: Aaron Marburg - -#include - -#include "marine_acoustic_msgs/ProjectedSonarImage.h" -#include "nodelet/nodelet.h" -#include "ros/ros.h" -#include "sonar_image_proc/sonar_image_msg_interface.h" - -namespace sonar_postprocessor { - -using marine_acoustic_msgs::ProjectedSonarImage; -using sonar_image_proc::SonarImageMsgInterface; - -class SonarPostprocessorNodelet : public nodelet::Nodelet { - public: - SonarPostprocessorNodelet() : Nodelet() { ; } - - virtual ~SonarPostprocessorNodelet() { ; } - - private: - virtual void onInit() { - ros::NodeHandle nh = getMTNodeHandle(); - ros::NodeHandle pnh = getMTPrivateNodeHandle(); - - pnh.param("gain", gain_, 1.0); - pnh.param("gamma", gamma_, 0.0); - - subSonarImage_ = nh.subscribe( - "sonar_image", 10, &SonarPostprocessorNodelet::sonarImageCallback, - this); - - pubSonarImage_ = - nh.advertise("sonar_image_postproc", 10); - - ROS_DEBUG("sonar_processor ready to run..."); - } - - void sonarImageCallback( - const marine_acoustic_msgs::ProjectedSonarImage::ConstPtr &msg) { - SonarImageMsgInterface interface(msg); - - // For now, only postprocess 32bit images - if (msg->image.dtype != msg->image.DTYPE_UINT32) { - pubSonarImage_.publish(msg); - return; - } - - // Expect this will copy - marine_acoustic_msgs::ProjectedSonarImage out = *msg; - - // For now, only 8-bit output is supported - out.image.dtype = out.image.DTYPE_UINT8; - out.image.data.clear(); - out.image.data.reserve(interface.ranges().size() * - interface.azimuths().size()); - - double logmin, logmax; - - for (unsigned int r_idx = 0; r_idx < interface.nRanges(); ++r_idx) { - for (unsigned int a_idx = 0; a_idx < interface.nAzimuth(); ++a_idx) { - sonar_image_proc::AzimuthRangeIndices idx(a_idx, r_idx); - - // const uint32_t pix = interface.intensity_uint32(idx); - // const auto range = interface.range(r_idx); - - // Avoid log(0) - auto intensity = interface.intensity_uint32(idx); - auto v = log(std::max((uint)1, intensity)) / log(UINT32_MAX); - - if ((r_idx == 0) && (a_idx == 0)) { - logmin = v; - logmax = v; - } else { - logmin = std::min(v, logmin); - logmax = std::max(v, logmax); - } - - const float vmax = 1.0, threshold = 0.74; - - v = (v - threshold) / (vmax - threshold); - v = std::min(1.0, std::max(0.0, v)); - out.image.data.push_back(UINT8_MAX * v); - - // Just do the math in float for now - // float i = static_cast(pix)/UINT32_MAX; - - // //ROS_INFO_STREAM(a_idx << "," << r_idx << " : " << pix << " => " << - // i); - - // i *= gain_; - - // UINT8_MAX * i); - } - } - - // - float dr = exp(logmax - logmin); - pubSonarImage_.publish(out); - } - - protected: - ros::Subscriber subSonarImage_; - ros::Publisher pubSonarImage_; - - float gain_, gamma_; -}; - -} // namespace sonar_postprocessor - -#include -PLUGINLIB_EXPORT_CLASS(sonar_postprocessor::SonarPostprocessorNodelet, - nodelet::Nodelet); diff --git a/ros/tools/bag2sonar.cpp b/ros/tools/bag2sonar.cpp deleted file mode 100644 index 275194a..0000000 --- a/ros/tools/bag2sonar.cpp +++ /dev/null @@ -1,176 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "sonar_image_proc/SonarDrawer.h" - -namespace po = boost::program_options; - -using std::string; -using std::vector; - -class OutputWrapper { - public: - virtual void write( - const marine_acoustic_msgs::ProjectedSonarImage::ConstPtr &msg, - const cv::Mat &mat) = 0; -}; - -class BagOutput : public OutputWrapper { - public: - BagOutput(const std::string &bagfile, const std::string topic) - : _bag(bagfile, rosbag::bagmode::Write), _topic(topic) {} - - void write(const marine_acoustic_msgs::ProjectedSonarImage::ConstPtr &msg, - const cv::Mat &mat) override { - cv_bridge::CvImage img_bridge(msg->header, - sensor_msgs::image_encodings::RGB8, mat); - - sensor_msgs::Image output_msg; - img_bridge.toImageMsg(output_msg); - - // Retain original message's timestamp - _bag.write(_topic, msg->header.stamp, output_msg); - } - - rosbag::Bag _bag; - std::string _topic; -}; - -void print_help(const po::options_description &description) { - std::cout << "Usage:" << std::endl; - std::cout << std::endl; - std::cout << " bag2sonar [options] " << std::endl; - std::cout << std::endl; - std::cout << description; - exit(0); -} - -int main(int argc, char **argv) { - po::options_description public_description("Draw sonar from a bagfile"); - - // The following code with three differente po::option_descriptions in a - // slight-of-hand to hide the positional argment "input-files" - // otherwise it shows up in print_help() which is ugly - // - // clang-format off - public_description.add_options() - ("help,h", "Display this help message") - ("logscale,l", po::bool_switch()->default_value(true), "Do logscale") - ("min-db", po::value()->default_value(0), "Min db") - ("max-db", po::value()->default_value(0), "Max db") - ("osd", po::bool_switch()->default_value(true), "If set, include the on-screen display in output") - ("output-bag,o", po::value(), "Name of output bagfile") - ("output-topic,t", po::value()->default_value("/drawn_sonar"), "Topic for images in output bagfile"); - - po::options_description hidden_description(""); - hidden_description.add_options() - ("input-files", po::value>()->required(), "Input files"); - // clang-format on - - po::options_description full_description(""); - full_description.add(public_description).add(hidden_description); - - po::positional_options_description p; - p.add("input-files", -1); - - po::variables_map vm; - - try { - po::store(po::command_line_parser(argc, argv) - .options(full_description) - .positional(p) - .run(), - vm); - po::notify(vm); - - } catch (const boost::program_options::required_option &e) { - // This exception occurs when a required option (input-files) isn't supplied - // Catch that and display a help message instead. - print_help(public_description); - } catch (const std::exception &e) { - // Catch any other random errors and exit - std::cerr << e.what() << std::endl; - exit(-1); - } - - if (vm.count("help")) { - print_help(public_description); - } else if (vm.count("input-files")) { - if (vm.count("input-files") > 1) { - std::cerr << "Can only process one file at a time" << std::endl; - exit(-1); - } - - std::vector> outputs; - - if (vm.count("output-bag")) { - outputs.push_back(std::make_shared( - vm["output-bag"].as(), vm["output-topic"].as())); - } - - sonar_image_proc::SonarDrawer sonar_drawer; - std::unique_ptr color_map( - new sonar_image_proc::InfernoColorMap); - - std::vector files = - vm["input-files"].as>(); - for (std::string file : files) { - std::cout << "Processing input file " << file << std::endl; - - rosbag::Bag bag(file, rosbag::bagmode::Read); - - std::cout << "Bagfile " << file << " is " << bag.getSize() << " bytes" - << std::endl; - - rosbag::View view( - bag, rosbag::TypeQuery("marine_acoustic_msgs/ProjectedSonarImage")); - - int count = 0; - - BOOST_FOREACH (rosbag::MessageInstance const m, view) { - marine_acoustic_msgs::ProjectedSonarImage::ConstPtr msg = - m.instantiate(); - - sonar_image_proc::SonarImageMsgInterface interface(msg); - if (vm["logscale"].as()) { - interface.do_log_scale(vm["min-db"].as(), - vm["max-db"].as()); - } - - cv::Mat rectMat = - sonar_drawer.drawRectSonarImage(interface, *color_map); - - cv::Mat sonarMat = sonar_drawer.remapRectSonarImage(interface, rectMat); - - cv::Mat outMat; - if (vm["osd"].as()) { - outMat = sonar_drawer.drawOverlay(interface, sonarMat); - } else { - outMat = sonarMat; - } - - for (auto output : outputs) { - output->write(msg, outMat); - } - - count++; - - if ((count % 100) == 0) { - std::cout << "Processed " << count << " sonar frames" << std::endl; - } - } - - bag.close(); - } - } - - exit(0); -} diff --git a/scripts/sonar_fov.py b/scripts/sonar_fov.py deleted file mode 100755 index 6c8d2d0..0000000 --- a/scripts/sonar_fov.py +++ /dev/null @@ -1,185 +0,0 @@ -#! /usr/bin/env python3 -""" -Copyright 2023 University of Washington Applied Physics Laboratory -Author: Marc Micatka & Laura Lindzey -""" -from __future__ import annotations # use type of class in member function annotation. -import numpy as np -import rospy - -from marine_acoustic_msgs.msg import ProjectedSonarImage -from geometry_msgs.msg import Point -from std_msgs.msg import Header -from visualization_msgs.msg import Marker, MarkerArray - -from sonar_image_proc.sonar_msg_metadata import SonarImageMetadata - - -def build_vector_list(msg_metadata): - """ - From sonar parameters, build the wedge as list of vectors - """ - xx = [] - yy = [] - zz = [] - - # Calculate ending arc of the FOV in 3D space - for elevation in [msg_metadata.min_elevation, msg_metadata.max_elevation]: - ce = np.cos(elevation) - se = np.sin(elevation) - ca = np.cos(msg_metadata.azimuths) - sa = np.sin(msg_metadata.azimuths) - - z = msg_metadata.max_range * ce * ca - y = -1 * msg_metadata.max_range * ce * sa - x = np.full_like(z, fill_value=msg_metadata.max_range * se) - xx.extend(x) - yy.extend(y) - zz.extend(z) - - # Add a point at the origin - xx_new = np.insert(np.array(xx), 0, 0) - yy_new = np.insert(np.array(yy), 0, 0) - zz_new = np.insert(np.array(zz), 0, 0) - - # Vertices are all the points on the edge of the wedge - # Points are at the origin and then at the end of every arc - vertices = np.zeros(shape=(len(xx_new), 3)) - vertices[:, 0] = xx_new - vertices[:, 1] = yy_new - vertices[:, 2] = zz_new - - # The vertices, faces, and connections are directional in nature - # Because the mesh is one-sided and will be invisible certain directions - # if the order is incorrect - - # Number of faces: - vertex_cnt = len(xx_new) + 1 - face_cnt = vertex_cnt // 2 - 2 - split = (len(xx_new) - 1) // 2 - - pt0 = np.zeros(shape=(face_cnt), dtype=np.uint) - pt1 = np.arange(1, face_cnt + 1, dtype=np.uint) - pt2 = pt1 + 1 - pt3 = pt2 + face_cnt - pt4 = pt3 + 1 - top_face = np.stack([pt0, pt1, pt2]).T - btm_face = np.stack([pt4, pt3, pt0]).T - - # Add triangles on sides: - left_face = np.array([[0, split + 1, 1]], dtype=np.uint) - right_face = np.array([[0, split, len(xx_new) - 1]], dtype=np.uint) - - # Add triangles between elevations: - first_pt = np.arange(1, split, dtype=np.uint) - second_pt = first_pt + 1 - third_pt = second_pt + split - 1 - fourth_pt = third_pt + 1 - - first_connection = np.stack([first_pt, third_pt, fourth_pt]).T - second_connection = np.stack([first_pt, fourth_pt, second_pt]).T - elevation_faces = np.vstack([first_connection, second_connection]) - - faces = np.vstack([top_face, btm_face, left_face, right_face, elevation_faces]) - - # Create array of vectors - vectors = [] - for f in faces: - for j in range(3): - x, y, z = vertices[f[j], :] - pt = Point(x, y, z) - vectors.append(pt) - return vectors - - -class SonarFOV: - def __init__(self): - self.sub = rospy.Subscriber( - "sonar_image", - ProjectedSonarImage, - self.sonar_image_callback, - queue_size=1, - buff_size=1000000, - ) - self.pub_fov = rospy.Publisher("sonar_fov", MarkerArray, queue_size=10) - - # Alpha transparency for wedge - self.alpha = rospy.get_param("~alpha", 1.0) - if isinstance(self.alpha, str): - self.alpha = float(self.alpha) - - # RGB color for wedge - self.color = rospy.get_param("~color", [0.0, 1.0, 0.0]) - if isinstance(self.color, str): - self.color = eval(self.color) - - self.vector_list = None - self.sonar_msg_metadata = None - - def generate_marker_array(self, vector_list) -> MarkerArray: - """ - Create MarkerArray from the pre-computed FOV wedge. - Sets the color and alpha from rosparams - """ - obj = Marker() - obj.type = Marker.TRIANGLE_LIST - obj.id = 1 - obj.points = vector_list - obj.frame_locked = True - - obj.scale.x = 1.0 - obj.scale.y = 1.0 - obj.scale.z = 1.0 - - obj.color.r = self.color[0] - obj.color.g = self.color[1] - obj.color.b = self.color[2] - obj.color.a = self.alpha - - wedge = MarkerArray() - wedge.markers = [obj] - return wedge - - def sonar_image_callback(self, sonar_image_msg: ProjectedSonarImage): - """ - Callback to publish the marker array containing the FOV wedge. - Only updates the geometry if parameters change. - """ - rospy.logdebug( - "Received new image, seq %d at %f" - % (sonar_image_msg.header.seq, sonar_image_msg.header.stamp.to_sec()) - ) - - # For the sonar_fov script, the elevation steps need to be 2 - # The logic to draw the mesh is not generic to step counts - new_metadata = SonarImageMetadata(sonar_image_msg) - generate_fov_flag = False - if self.sonar_msg_metadata is None: - self.sonar_msg_metadata = new_metadata - generate_fov_flag = True # generate the fov stl - else: - # Because the dictionary contains numpy arrays, a simple check for a == b does not work. - # Using allclose because the range occasionally changes by fractions - if ( - self.sonar_msg_metadata is None - or self.sonar_msg_metadata != new_metadata - ): - # things have changed, regenerate the fov - rospy.logdebug("Updating Parameters of FOV") - self.sonar_msg_metadata = new_metadata - generate_fov_flag = True - - if generate_fov_flag: - rospy.logdebug("Generating FOV mesh...") - self.vector_list = build_vector_list(self.sonar_msg_metadata) - - wedge = self.generate_marker_array(self.vector_list) - for obj in wedge.markers: - obj.header = sonar_image_msg.header - self.pub_fov.publish(wedge) - - -if __name__ == "__main__": - rospy.init_node("sonar_fov") - fov_publisher = SonarFOV() - rospy.spin() diff --git a/scripts/sonar_pointcloud.py b/scripts/sonar_pointcloud.py deleted file mode 100755 index df90cef..0000000 --- a/scripts/sonar_pointcloud.py +++ /dev/null @@ -1,291 +0,0 @@ -#! /usr/bin/env python3 -""" -Copyright 2023 University of Washington Applied Physics Laboratory -Author: Marc Micatka & Laura Lindzey -""" - -from __future__ import annotations # use type of class in member function annotation. -from matplotlib import cm -import numpy as np -import rospy -import time -from marine_acoustic_msgs.msg import ProjectedSonarImage, SonarImageData -from sensor_msgs.msg import PointCloud2, PointField -from std_msgs.msg import Header - -from sonar_image_proc.sonar_msg_metadata import SonarImageMetadata - - -def make_geometry(sonar_msg_metadata: SonarImageMetadata, elevations) -> np.array: - """ - Vectorized geometry generation. - Regenerates when there are changing parameters from the sonar. - - The original make_geometry() fxn was nested loops. We replaced it with a vectorized solution but - for the purposes of making the indexing and geometry creation more clear, - here is the original implementation: - - ces = np.cos(elevations) - ses = np.sin(elevations) - cas = np.cos(self.azimuths) - sas = np.sin(self.azimuths) - for kk, (ce, se) in enumerate(zip(ces, ses)): - for ii, (ca, sa) in enumerate(zip(cas, sas)): - for jj, distance in enumerate(self.ranges): - idx = ii + jj * self.num_angles - zz = distance * ce * ca - yy = distance * ce * sa - xx = distance * se - points[kk][idx] = [xx, yy, zz] - """ - rospy.logdebug("Making Geometry") - begin_time = time.time() - # Pre-compute these values to speed up the loop - # Compute the idxs to properly index into the points array - idxs = np.arange(0, sonar_msg_metadata.num_angles * sonar_msg_metadata.num_ranges) - idxs = idxs.reshape( - sonar_msg_metadata.num_ranges, sonar_msg_metadata.num_angles - ).flatten(order="F") - - # Compute the cosines and sines of elevations and azimuths - ces = np.cos(elevations) - ses = np.sin(elevations) - cas = np.cos(sonar_msg_metadata.azimuths) - sas = np.sin(sonar_msg_metadata.azimuths) - - # Allocate points - new_shape = ( - len(elevations), - sonar_msg_metadata.num_ranges * sonar_msg_metadata.num_angles, - 3, - ) - points = np.zeros(shape=(new_shape)) - - x_temp = np.tile( - sonar_msg_metadata.ranges[np.newaxis, :] * ses[:, np.newaxis], - reps=sonar_msg_metadata.num_angles, - ).flatten() - y_temp = ( - sonar_msg_metadata.ranges[np.newaxis, np.newaxis, :] - * ces[:, np.newaxis, np.newaxis] - * sas[np.newaxis, :, np.newaxis] - ).flatten() - z_temp = ( - sonar_msg_metadata.ranges[np.newaxis, np.newaxis, :] - * ces[:, np.newaxis, np.newaxis] - * cas[np.newaxis, :, np.newaxis] - ).flatten() - - points[:, idxs, :] = np.stack([x_temp, y_temp, z_temp], axis=1).reshape(new_shape) - - total_time = time.time() - begin_time - rospy.logdebug(f"Creating geometry took {1000*total_time:0.2f} ms") - return points - - -def make_color_lookup() -> np.array: - """ - Generates a lookup table from matplotlib inferno colormapping - """ - color_lookup = np.zeros(shape=(256, 4)) - - for aa in range(256): - r, g, b, _ = cm.inferno(aa) - alpha = aa / 256 - - color_lookup[aa, :] = [r, g, b, alpha] - return color_lookup - - -class SonarPointcloud(object): - """ - Subscribe to ProjectedSonarImage messages and publish slices of the - intensity array as rviz MarkerArrays. - """ - - def __init__(self): - # NB: if queue_size is set, have to be sure buff_size is sufficiently large, - # since it can only discard messages that are fully in the buffer. - # https://stackoverflow.com/questions/26415699/ros-subscriber-not-up-to-date - # (200k didn't work, and sys.getsizeof doesn't return an accurate size of the whole object) - self.sub = rospy.Subscriber( - "sonar_image", - ProjectedSonarImage, - self.sonar_image_callback, - queue_size=1, - buff_size=1000000, - ) - self.pub = rospy.Publisher("sonar_cloud", PointCloud2, queue_size=1) - - # Flag to determine whether we publish ALL points or only non-zero points - self.publish_all_points = rospy.get_param("~publish_all_points", False) - - # cmin is a color scaling parameter. The log-scaled and normed intensity values - # get scaled to (cmin, 1.0) - self.cmin = rospy.get_param("~cmin", 0.74) - - # threshold is the publish parameter if publish_all_points is false - self.threshold = rospy.get_param("~threshold", 0) - - # Number of planes to add to the pointcloud projection. - # Steps are evenly distributed between the max and min elevation angles. - elev_steps = rospy.get_param("~elev_steps", 2) - if isinstance(elev_steps, (float, str)): - elev_steps = int(elev_steps) - min_elev_deg = rospy.get_param("~min_elev_deg", -10) - max_elev_deg = rospy.get_param("~max_elev_deg", 10) - assert max_elev_deg >= min_elev_deg - min_elev = np.radians(min_elev_deg) - max_elev = np.radians(max_elev_deg) - self.elevations = np.linspace(min_elev, max_elev, elev_steps) - - # x,y,z coordinates of points - # [ELEVATION_IDX, INTENSITY_IDX, DIMENSION_IDX] - # Shape chosen for ease of mapping coordinates to intensities - self.geometry = None - - # We need to detect whether the geometry has changed. - self.sonar_msg_metadata = None - - self.color_lookup = None - self.output_points = None - - def normalize_intensity_array(self, image: SonarImageData): - """ - Process an intensity array into a parseable format for pointcloud generation - Can handle 8bit or 32bit input data, will log scale output data - - Input intensities are on the range [0, INT_MAX]. - Output intensities are log-scaled, normalized to [0, 1] - """ - if image.dtype == image.DTYPE_UINT8: - data_type = np.uint8 - elif image.dtype == image.DTYPE_UINT32: - data_type = np.uint32 - else: - raise Exception("Only 8 bit and 32 bit data is supported!") - - intensities = np.frombuffer(image.data, dtype=data_type) - new_intensites = intensities.astype(np.float32) - # Scaling calculation is taken from sonar_image_proc/ros/src/sonar_postprocessor_nodelet.cpp - # First we log scale the intensities - - # Avoid log(0) and scale full range of possible intensities to [0,1] - vv = np.log(np.maximum(1, new_intensites)) / np.log(np.iinfo(data_type).max) - return vv - - def sonar_image_callback(self, sonar_image_msg: ProjectedSonarImage): - """ - Convert img_msg into point cloud with color mappings via numpy. - """ - begin_time = time.time() - rospy.logdebug( - "Received new image, seq %d at %f" - % (sonar_image_msg.header.seq, sonar_image_msg.header.stamp.to_sec()) - ) - header = Header() - header = sonar_image_msg.header - - frame_id = rospy.get_param("~frame_id", None) - if frame_id: - header.frame_id = frame_id - - fields = [ - PointField("x", 0, PointField.FLOAT32, 1), - PointField("y", 4, PointField.FLOAT32, 1), - PointField("z", 8, PointField.FLOAT32, 1), - PointField("r", 12, PointField.FLOAT32, 1), - PointField("g", 16, PointField.FLOAT32, 1), - PointField("b", 20, PointField.FLOAT32, 1), - PointField("a", 24, PointField.FLOAT32, 1), - ] - - new_metadata = SonarImageMetadata(sonar_image_msg) - if self.sonar_msg_metadata is None or self.sonar_msg_metadata != new_metadata: - rospy.logdebug( - f"Metadata updated! \n" - f"Old: {self.sonar_msg_metadata} \n" - f"New: {new_metadata}" - ) - - self.sonar_msg_metadata = new_metadata - self.geometry = make_geometry(self.sonar_msg_metadata, self.elevations) - - if self.color_lookup is None: - self.color_lookup = make_color_lookup() - t0 = time.time() - - normalized_intensities = self.normalize_intensity_array(sonar_image_msg.image) - - # Threshold pointcloud based on intensity if not publishing all points - if self.publish_all_points: - selected_intensities = normalized_intensities - geometry = self.geometry - else: - pos_intensity_idx = np.where(normalized_intensities > self.threshold) - selected_intensities = normalized_intensities[pos_intensity_idx] - geometry = self.geometry[:, pos_intensity_idx[0]] - - num_original = len(normalized_intensities) - num_selected = len(selected_intensities) - rospy.logdebug( - f"Filtering Results: (Pts>Thresh, Total): {num_selected, num_original}. " - f"Frac: {(num_selected/num_original):.3f}" - ) - - # Finally, apply colormap to intensity values. - v_max = 1.0 - colors = (selected_intensities - self.cmin) / (v_max - self.cmin) - c_clipped = np.clip(colors, a_min=0.0, a_max=1.0) - c_uint8 = (np.iinfo(np.uint8).max * c_clipped).astype(np.uint8) - - # Allocate our output points - npts = len(selected_intensities) - self.output_points = np.zeros( - (len(self.elevations) * npts, 7), dtype=np.float32 - ) - - # Expand out intensity array (for fast comparison) - # The np.where call setting colors requires an array of shape (npts, 4). - # selected_intensities has shape (npts,), but np.repeat requires (npts, 1). - c_expanded = np.repeat(c_uint8[..., np.newaxis], 4, axis=1) - elev_points = np.empty((npts, 7)) - elev_points[:, 3:] = self.color_lookup[c_expanded[:, 0]] - - # Fill the output array - for i in range(len(self.elevations)): - elev_points[:, 0:3] = geometry[i, :, :] - step = i * npts - next_step = step + npts - self.output_points[step:next_step, :] = elev_points - - t1 = time.time() - n_pub = len(self.output_points) - cloud_msg = PointCloud2( - header=header, - height=1, - width=n_pub, - is_dense=True, - is_bigendian=False, - fields=fields, - point_step=7 * 4, - row_step=7 * 4 * n_pub, - data=self.output_points.tobytes(), - ) - - dt1 = time.time() - t1 - dt0 = t1 - t0 - total_time = time.time() - begin_time - self.pub.publish(cloud_msg) - - rospy.logdebug( - f"published pointcloud: npts = {npts}, Find Pts = {dt0:0.5f} sec, " - f"Convert to Cloud = {dt1:0.5f} sec. " - f"Total Time = {total_time:0.3f} sec" - ) - - -if __name__ == "__main__": - rospy.init_node("sonar_pointcloud") - pointcloud_publisher = SonarPointcloud() - rospy.spin() diff --git a/setup.py b/setup.py index 2f5941f..16d148e 100644 --- a/setup.py +++ b/setup.py @@ -5,8 +5,8 @@ from distutils.core import setup setup( - version='0.0.1', - scripts=['python/histogram_drawer'], - packages=['sonar_image_proc'], - package_dir={'': 'python/'} + version="0.0.1", + scripts=["python/histogram_drawer"], + packages=["sonar_image_proc"], + package_dir={"": "python/"}, ) diff --git a/sonar_image_proc.rosinstall b/sonar_image_proc.rosinstall deleted file mode 100644 index f1f7beb..0000000 --- a/sonar_image_proc.rosinstall +++ /dev/null @@ -1 +0,0 @@ -- git: {local-name: hydrographic_msgs, uri: "https://github.com/apl-ocean-engineering/hydrographic_msgs.git"} diff --git a/test/unit/test_draw_sonar.cpp b/test/unit/test_draw_sonar.cpp index 7931dec..bf33e96 100644 --- a/test/unit/test_draw_sonar.cpp +++ b/test/unit/test_draw_sonar.cpp @@ -5,7 +5,7 @@ #include using namespace std; -#include "sonar_image_proc/DrawSonar.h" -using namespace sonar_image_proc; +#include "libdrawsonar/DrawSonar.h" +using namespace libdrawsonar; TEST(TestDrawSonar, NullTest) { ASSERT_TRUE(true); }