diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..64957be --- /dev/null +++ b/.clang-format @@ -0,0 +1,35 @@ +--- +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: DontAlign +AlignOperands: AlignAfterOperator +AlignTrailingComments: false +# AllowAllParametersOfDeclarationOnNextLine: true +AllowShortFunctionsOnASingleLine: Inline +AllowShortIfStatementsOnASingleLine: Never +BinPackParameters: false +ColumnLimit: 130 +ConstructorInitializerIndentWidth: 2 +DerivePointerAlignment: false +IncludeCategories: + - Regex: '<[^\/]+\.h>' + Priority: 1 + - Regex: '<.*\.h>' + Priority: 2 + - Regex: '<[^\/\.]+\/[^\.]+>' + Priority: 3 + - Regex: '<[^\/\.]+>' + Priority: 4 + - Regex: '<.*\.hpp>' + Priority: 5 + - Regex: '"[^\/]+\.h"' + Priority: 6 + - Regex: '".*\.h"' + Priority: 7 + - Regex: '".*\.hpp"' + Priority: 8 +SeparateDefinitionBlocks: Always +Standard: c++17 +TabWidth: 2 +... diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000..8f85db3 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,179 @@ +Checks: > + -*, + bugprone-argument-comment, + bugprone-assert-side-effect, + bugprone-bad-signal-to-kill-thread, + bugprone-branch-clone, + bugprone-copy-constructor-init, + bugprone-dangling-handle, + bugprone-dynamic-static-initializers, + bugprone-fold-init-type, + bugprone-forward-declaration-namespace, + bugprone-forwarding-reference-overload, + bugprone-inaccurate-erase, + bugprone-incorrect-roundings, + bugprone-integer-division, + bugprone-lambda-function-name, + bugprone-macro-parentheses, + bugprone-macro-repeated-side-effects, + bugprone-misplaced-operator-in-strlen-in-alloc, + bugprone-misplaced-pointer-arithmetic-in-alloc, + bugprone-misplaced-widening-cast, + bugprone-move-forwarding-reference, + bugprone-multiple-statement-macro, + bugprone-narrowing-conversions, + bugprone-no-escape, + bugprone-not-null-terminated-result, + bugprone-parent-virtual-call, + bugprone-posix-return, + bugprone-reserved-identifier, + bugprone-sizeof-container, + bugprone-sizeof-expression, + bugprone-string-constructor, + bugprone-string-integer-assignment, + bugprone-string-literal-with-embedded-nul, + bugprone-suspicious-enum-usage, + bugprone-suspicious-include, + bugprone-suspicious-memory-comparison, + bugprone-suspicious-memset-usage, + bugprone-suspicious-missing-comma, + bugprone-suspicious-semicolon, + bugprone-suspicious-string-compare, + bugprone-swapped-arguments, + bugprone-terminating-continue, + bugprone-throw-keyword-missing, + bugprone-too-small-loop-variable, + bugprone-undefined-memory-manipulation, + bugprone-undelegated-constructor, + bugprone-unhandled-self-assignment, + bugprone-unused-raii, + bugprone-unused-return-value, + bugprone-use-after-move, + bugprone-virtual-near-miss, + boost-use-to-string, + cert-dcl03-c, + cert-dcl21-cpp, + cert-dcl58-cpp, + cert-err34-c, + cert-err52-cpp, + cert-err58-cpp, + cert-err60-cpp, + cert-flp30-c, + cert-msc50-cpp, + cert-msc51-cpp, + cert-oop54-cpp, + cert-str34-c, + cppcoreguidelines-interfaces-global-init, + cppcoreguidelines-narrowing-conversions, + cppcoreguidelines-pro-type-member-init, + cppcoreguidelines-pro-type-static-cast-downcast, + cppcoreguidelines-slicing, + google-default-arguments, + google-explicit-constructor, + google-runtime-operator, + hicpp-exception-baseclass, + hicpp-multiway-paths-covered, + hicpp-signed-bitwise, + misc-misplaced-const, + misc-new-delete-overloads, + misc-no-recursion, + misc-non-copyable-objects, + misc-redundant-expression, + misc-static-assert, + misc-throw-by-value-catch-by-reference, + misc-unconventional-assign-operator, + misc-uniqueptr-reset-release, + modernize-avoid-bind, + modernize-concat-nested-namespaces, + modernize-deprecated-headers, + modernize-deprecated-ios-base-aliases, + modernize-loop-convert, + modernize-make-shared, + modernize-make-unique, + modernize-pass-by-value, + modernize-raw-string-literal, + modernize-redundant-void-arg, + modernize-replace-auto-ptr, + modernize-replace-disallow-copy-and-assign-macro, + modernize-replace-random-shuffle, + modernize-return-braced-init-list, + modernize-shrink-to-fit, + modernize-unary-static-assert, + modernize-use-auto, + modernize-use-bool-literals, + modernize-use-emplace, + modernize-use-equals-default, + modernize-use-equals-delete, + modernize-use-nodiscard, + modernize-use-noexcept, + modernize-use-nullptr, + modernize-use-override, + modernize-use-transparent-functors, + modernize-use-uncaught-exceptions, + mpi-buffer-deref, + mpi-type-mismatch, + openmp-use-default-none, + performance-faster-string-find, + performance-for-range-copy, + performance-implicit-conversion-in-loop, + performance-inefficient-algorithm, + performance-inefficient-string-concatenation, + performance-inefficient-vector-operation, + performance-move-const-arg, + performance-move-constructor-init, + performance-no-automatic-move, + performance-noexcept-move-constructor, + performance-trivially-destructible, + performance-type-promotion-in-math-fn, + performance-unnecessary-copy-initialization, + performance-unnecessary-value-param, + portability-simd-intrinsics, + readability-avoid-const-params-in-decls, + readability-const-return-type, + readability-container-size-empty, + readability-convert-member-functions-to-static, + readability-delete-null-pointer, + readability-deleted-default, + readability-identifier-naming, + readability-inconsistent-declaration-parameter-name, + readability-magic-numbers, + readability-make-member-function-const, + readability-misleading-indentation, + readability-misplaced-array-index, + readability-non-const-parameter, + readability-redundant-control-flow, + readability-redundant-declaration, + readability-redundant-function-ptr-dereference, + readability-redundant-smartptr-get, + readability-redundant-string-cstr, + readability-redundant-string-init, + readability-simplify-subscript-expr, + readability-static-accessed-through-instance, + readability-static-definition-in-anonymous-namespace, + readability-string-compare, + readability-uniqueptr-delete-release, + readability-use-anyofallof + +WarningsAsErrors: > + readability-magic-numbers, + readability-identifier-naming + +CheckOptions: + - { key: readability-identifier-naming.NamespaceCase, value: aNy_CasE } + - { key: readability-identifier-naming.ClassCase, value: CamelCase } + - { key: readability-identifier-naming.StructCase, value: CamelCase } + - { key: readability-identifier-naming.EnumCase, value: CamelCase } + - { key: readability-identifier-naming.TemplateParameterCase, value: CamelCase } + - { key: readability-identifier-naming.UnionCase, value: CamelCase } + - { key: readability-identifier-naming.FunctionCase, value: camelBack } + - { key: readability-identifier-naming.MethodCase, value: camelBack } + - { key: readability-identifier-naming.VariableCase, value: lower_case } + - { key: readability-identifier-naming.ClassMemberCase, value: lower_case } + - { key: readability-identifier-naming.PrivateMemberSuffix, value: _ } + - { key: readability-identifier-naming.ProtectedMemberSuffix, value: _ } + - { key: readability-identifier-naming.EnumConstantCase, value: UPPER_CASE } + - { key: readability-identifier-naming.ConstexprVariableCase, value: UPPER_CASE } + - { key: readability-identifier-naming.GlobalConstantCase, value: UPPER_CASE } + - { key: readability-identifier-naming.MemberConstantCase, value: UPPER_CASE } + - { key: readability-identifier-naming.StaticConstantCase, value: UPPER_CASE } + - { key: readability-magic-numbers.IgnoredIntegerValues, value: '1;2;3;4;5;6' } diff --git a/.codespell_words b/.codespell_words new file mode 100644 index 0000000..7674eb5 --- /dev/null +++ b/.codespell_words @@ -0,0 +1,18 @@ +forssea +frs +FRS +oculus +ros +ros2 +rosgraph +rostopic +rosservice +rosnode +rosaction +ros2action +ros2bag +rosbag +ros2node +ros2topic +sim +hmi diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..669ee1b --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "oculus_driver/oculus_driver"] + path = oculus_driver/oculus_driver + url = https://github.com/forssea-robotics/oculus_driver.git + branch = feature/ROB-378/oculus_sonar # TODO(abourgois, branch to handle) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..dbdcbef --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,80 @@ +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.1.0 + hooks: + - id: check-added-large-files + - id: check-case-conflict + - id: check-json + - id: check-merge-conflict + - id: check-symlinks + - id: check-toml + - id: check-xml + - id: check-yaml + - id: check-ast + - id: check-docstring-first + - id: debug-statements + - id: destroyed-symlinks + - id: detect-private-key + - id: end-of-file-fixer + - id: mixed-line-ending + entry: mixed-line-ending --fix=lf + - id: pretty-format-json + entry: pretty-format-json --autofix --indent 2 + - id: trailing-whitespace + - id: fix-byte-order-marker + - id: check-executables-have-shebangs + - id: check-shebang-scripts-are-executable + + - repo: https://github.com/psf/black + rev: 22.3.0 + hooks: + - id: black + + - repo: local + hooks: + - id: clang-format + name: clang-format + description: Format files with ClangFormat. + entry: clang-format -fallback-style=none -i + language: system + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ + + - repo: https://github.com/codespell-project/codespell + rev: v2.0.0 + hooks: + - id: codespell + args: ['--write-changes', '--ignore-words=.codespell_words'] + exclude: CHANGELOG.rst + + - repo: local + hooks: + - id: ament_cppcheck + name: ament_cppcheck + description: Static code analysis of C/C++ files. + stages: [commit] + entry: /usr/bin/bash -c 'source /opt/ros/humble/setup.bash && env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck' + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + + # Maybe use https://github.com/cpplint/cpplint instead + - repo: local + hooks: + - id: ament_cpplint + name: ament_cpplint + description: Static code analysis of C/C++ files. + stages: [commit] + entry: /usr/bin/bash -c 'source /opt/ros/humble/setup.bash && ament_cpplint --linelength=140 --filter=-whitespace/newline' + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + + # Cmake hooks + - repo: local + hooks: + - id: ament_lint_cmake + name: ament_lint_cmake + description: Check format of CMakeLists.txt files. + stages: [commit] + entry: /usr/bin/bash -c 'source /opt/ros/humble/setup.bash && ament_lint_cmake' + language: system + files: CMakeLists\.txt$ diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..57d340e --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any contribution that you make to this repository +will be under the BSD 3-Clause License Copyright +(c) 2022, ENSTA-Bretagne. diff --git a/README.md b/README.md index 2c3aaa8..f34afd9 100644 --- a/README.md +++ b/README.md @@ -7,14 +7,13 @@ ROS2 node for the Blueprint Subsea Oculus sonar. This is a ROS2 metapackage including: * A ROS2 package **oculus_interfaces** containing the useful ROS messages definitions, * A ROS2 package **oculus_ros2** interfacing the driver messages with ROS2 topics, - * A ROS2 package **oculus_image_converter** converting the sonar raw images into a fan-shaped view and publishing them on a ROS2 topic (TO DO @estellearrc). ## Requirements This ROS2 metapackage was developed and tested using: -* Ubuntu 20.04 LTS -* ROS2 galactic -* CMake 3.23 +* Ubuntu 22.04 LTS +* ROS2 humble +* CMake 3.22.1 ## Getting started @@ -42,54 +41,86 @@ cd .. && colcon build source install/setup.bash ``` -### Installation (without an internet connection) +## Installation (without an internet connection) -#### Install the oculus_driver library +### Install the oculus_driver library -If you don't have an internet connection available on the system on which you -want to use this node, your have to install the -[oculus_driver](https://github.com/ENSTABretagneRobotics/oculus_driver.git) -library beforehand. +github : https://github.com/ENSTABretagneRobotics/oculus_driver -Clone or copy the oculus_driver library : +This library follows a standard CMake compilation procedure. To use oculus_driver you have two possibility : + +#### Let colcon handle the dependency +In the packages CMakeList.txt the lines: +```cmake +if(NOT TARGET oculus_driver) + include(FetchContent) + FetchContent_Declare(oculus_driver + GIT_REPOSITORY https://github.com/ENSTABretagneRobotics/oculus_driver.git + GIT_TAG master # tag for development : TODO(hugoyvrn ?) handle the good version + ) + FetchContent_MakeAvailable(oculus_driver) +endif() ``` +clone the github repository during the `colcon build` compilation. + +#### Clone your self the oculus_driver library + +In your install directory in ``(for example: `~/work/my_user`) clone the repository: +```bash git clone https://github.com/ENSTABretagneRobotics/oculus_driver.git +cd oculus_driver +git switch master # for development ``` +handle the git branch as you want. Please keep in mind you have to pull the repository yourself to keep up to date. -This library follows a standard CMake compilation procedure. cd into the repo -and create a build directory : - +Create a build directory in the root of the repository : +```bash +mkdir build && cd build +``` +Make sure the CMAKE_PREFIX_PATH environment variable contains your install +location : +```bash +echo $CMAKE_PREFIX_PATH ``` -cd oculus_driver && mkdir build && cd build + +If not, put this at the end your `$HOME/.bashrc` file: +```bash +export CMAKE_PREFIX_PATH=:$CMAKE_PREFIX_PATH ``` Generate your build system with CMake, compile and install : -``` +```bash cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX= .. make -j4 install ``` -Make sure the CMAKE_PREFIX_PATH environment variable contains your install -location : -``` -echo $CMAKE_PREFIX_PATH + +#### install the python package +Install `pybind11` with `pip` : +```bash +cd ../python # if you are in oculus_driver/build +pip3 install --upgrade pip +pip3 install --upgrade "pybind11[global]" ``` -If not, put this at the end your $HOME/.bashrc file: +Go in `/oculus_driver/python`, make sure ROS is NOT sourced and run +```bash +echo $ROS_DISTRO # Must be empty +rm -r build/ oculus_python.egg-info/ oculus_python.*.so # if needed ``` -export CMAKE_PREFIX_PATH=:$CMAKE_PREFIX_PATH +```bash +pip3 install --user -e . # make takes few seconds +``` +To use the library you will need to export before building with colcon : +```bash +export LD_LIBRARY_PATH=:$LD_LIBRARY_PATH ``` - -Now follow the oculus_ros2 node installation procedure normally. If the -oculus_driver is properly installed **at a location included in the -CMAKE_PREFIX_PATH environment variable**. The node should compile properly. - ## Using the oculus_ros2 node Launch ROS2 node with your listening port for sonar data (default 52102): ``` -ros2 launch oculus_ros2 default.launch.py -port +ros2 launch oculus_ros2 default.launch.py ``` **N.B.** Remap topics to change their name in the launch file. @@ -118,7 +149,7 @@ ros2 param list ``` To get a detailed description of a param, for example *ping_rate*, use: ``` -ros2 param describe /oculus_sonar ping_rate +ros2 param describe /oculus_sonar ping_rate ``` And you get: ``` @@ -140,7 +171,7 @@ Parameter name: ping_rate To dynamically reconfigure the sonar parameters, use the RQT Dynamic Reconfigure GUI, or use the following example command line: ``` -ros2 param set /oculus_sonar gain_assist true +ros2 param set /oculus_sonar gain_assist false ``` The sonar might take a lot of time to acknowledge a parameter change (especially @@ -178,6 +209,3 @@ on Ubuntu). Most of the issue you ~~might~~ will encounter are related to network setup. Check the Network configuration section. - - - diff --git a/TODO b/TODO new file mode 100644 index 0000000..c6897ae --- /dev/null +++ b/TODO @@ -0,0 +1,89 @@ +# Including a filter +```c++ +// function from https://github.com/JaouadROS jaouad@forssea-robotics.fr +// void SonarViewer::streamAndFilter(const int &width, +// const int &height, +// const int &offset, +// const std::vector &ping_data, +// cv::Mat &data) +void SonarViewer::streamAndFilter(const oculus::PingMessage::ConstPtr& ping, cv::Mat& data) { + const int width = ping->bearing_count(); + const int step = width + SIZE_OF_GAIN_; + const int height = ping->range_count(); + const int offset = ping->ping_data_offset(); + + cv::Mat rawDataMat = cv::Mat(height, step, CV_8U); +#pragma omp parallel for collapse(2) + for (int i = 0, k = offset; i < height; i++) { + for (int j = 0; j < step; j++) { + rawDataMat.at(i, j) = ping->data()[k++]; + } + } + + data = cv::Mat(height, width, CV_64F); +#pragma omp parallel for collapse(2) + for (int i = 0; i < height; i++) { + for (int j = SIZE_OF_GAIN_; j < step; j++) { + data.at(i, j - SIZE_OF_GAIN_) = rawDataMat.at(i, j); + } + } + + cv::Mat img = data; + img = img / 255.0; + + cv::Mat img_f; + cv::dft(img, img_f, cv::DFT_COMPLEX_OUTPUT); + + cv::Mat beam(1, img.cols, CV_64F, cv::Scalar::all(0)); + + // set the values of the beam // TODO(hugoyvrn, to link it with ping data) + const int kNumValues = img.cols / 20; // TODO(JaouadROS, 20 is a magic number, what is it?) + double values[kNumValues]; + for (int i = 0; i < kNumValues / 2; i++) values[i] = 24 + i; // TODO(JaouadROS, 24 is a magic number, what is it?) + values[kNumValues / 2] = 70; // TODO(JaouadROS, 70 is a magic number, what is it?) + for (int i = kNumValues / 2 + 1; i < kNumValues; i++) values[i] = values[kNumValues - i - 1]; + for (int i = 0; i < kNumValues; i++) beam.at(0, i * img.cols / kNumValues) = values[i]; + + // normalize the beam + cv::Mat psf = (1.0 / cv::sum(beam)[0]) * beam; + + const int kw = psf.rows; + const int kh = psf.cols; + cv::Mat psf_padded = cv::Mat::zeros(img.size(), img.type()); + psf.copyTo(psf_padded(cv::Rect(0, 0, kh, kw))); + + // compute (padded) psf's DFT + cv::Mat psf_f; + cv::dft(psf_padded, psf_f, cv::DFT_COMPLEX_OUTPUT, kh); + + cv::Mat psf_f_2; + cv::pow(psf_f, 2, psf_f_2); + cv::transform(psf_f_2, psf_f_2, cv::Matx12f(1, 1)); + + const double noise = 0.001; + cv::Mat ipsf_f(psf_f.size(), CV_64FC2); + for (int i = 0; i < psf_f.rows; i++) { + for (int j = 0; j < psf_f.cols; j++) { + // compute element-wise division + cv::Vec2d val = psf_f.at(i, j) / (psf_f_2.at(i, j) + noise); + // store result in ipsf_f + ipsf_f.at(i, j) = val; + } + } + cv::Mat result_f; + cv::mulSpectrums(img_f, ipsf_f, result_f, 0); + + cv::Mat result; + cv::idft(result_f, result, cv::DFT_SCALE | cv::DFT_REAL_OUTPUT); + + cv::Mat result_shifted_rows(result.size(), result.type()); + int shift = ceil(kw / 2.0); + // similar to roll with shift = -1 + for (int i = 0; i < result.rows; i++) result.row(i).copyTo(result_shifted_rows.row((i + shift) % result_shifted_rows.rows)); + + shift = ceil(kh / 2.0); + for (int i = 0; i < result.cols; i++) result_shifted_rows.col(i).copyTo(result.col((i + shift) % result.cols)); + result.setTo(0, result < 0); + cv::normalize(result, result, 0, 1, cv::NORM_MINMAX); +} +``` diff --git a/oculus_interfaces/CMakeLists.txt b/oculus_interfaces/CMakeLists.txt index 5c8a1b6..b124aad 100644 --- a/oculus_interfaces/CMakeLists.txt +++ b/oculus_interfaces/CMakeLists.txt @@ -16,6 +16,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/OculusFireConfig.msg" "msg/OculusPing.msg" "msg/OculusStampedPing.msg" + "msg/Ping.msg" DEPENDENCIES builtin_interfaces std_msgs ) diff --git a/oculus_interfaces/msg/OculusHeader.msg b/oculus_interfaces/msg/OculusHeader.msg index bb27bc4..10372dd 100644 --- a/oculus_interfaces/msg/OculusHeader.msg +++ b/oculus_interfaces/msg/OculusHeader.msg @@ -4,4 +4,4 @@ uint16 dst_device_id uint16 msg_id uint16 msg_version uint32 payload_size -uint16 spare2 \ No newline at end of file +uint16 spare2 diff --git a/oculus_interfaces/msg/OculusPing.msg b/oculus_interfaces/msg/OculusPing.msg index c7380d4..e877dc7 100644 --- a/oculus_interfaces/msg/OculusPing.msg +++ b/oculus_interfaces/msg/OculusPing.msg @@ -17,4 +17,3 @@ uint32 image_size uint32 message_size uint8[] data - diff --git a/oculus_interfaces/msg/OculusStatus.msg b/oculus_interfaces/msg/OculusStatus.msg index 40e8df2..59b6fb4 100644 --- a/oculus_interfaces/msg/OculusStatus.msg +++ b/oculus_interfaces/msg/OculusStatus.msg @@ -5,7 +5,7 @@ uint16 device_type uint16 part_number uint32 status -OculusVersionInfo versin_info +OculusVersionInfo version_info uint32 ip_addr uint32 ip_mask diff --git a/oculus_interfaces/msg/OculusVersionInfo.msg b/oculus_interfaces/msg/OculusVersionInfo.msg index db3c6a9..e18253a 100644 --- a/oculus_interfaces/msg/OculusVersionInfo.msg +++ b/oculus_interfaces/msg/OculusVersionInfo.msg @@ -4,4 +4,3 @@ uint32 firmware_version1 uint32 firmware_date1 uint32 firmware_version2 uint32 firmware_date2 - diff --git a/oculus_interfaces/msg/Ping.msg b/oculus_interfaces/msg/Ping.msg new file mode 100644 index 0000000..97f9369 --- /dev/null +++ b/oculus_interfaces/msg/Ping.msg @@ -0,0 +1,44 @@ +std_msgs/Header header + +uint32 ping_id # incrementing counter inside the sonar +uint32 ping_firing_date # Ping firing date (sonar internal clock, microseconds) + # /!\ Will overlap in a bit more than 1h. The sonar + # itself gives this value in 32bits. Maximum + # number of usec in 32bits gives ~ 1h. + +float64 range # Maximum range value in this ping +float64 gain_percent # Percentage of gain (not documented) + +float64 frequency # Ping acoustic frequency (Hz) +float64 speed_of_sound_used # Speed of sound used by the sonar for range calculations (m/s) +float64 range_resolution # Distance between 2 rows in the ping image data. + + +float64 temperature # External temperature (C) +float64 pressure # External pressure (bar) + +uint8 master_mode # 1 is "low frequency" (1.2MHz), 2 is high frequency (2.1 MHz) +bool has_gains # Each row in the image data comes with a gain (to + # be compensated for consistent acoustic readings. + # See documentation for more details) + +uint16 n_ranges # Height of the ping image data. +uint16 n_beams # Width of the ping image data. +uint32 step # Size in bytes of each row in the ping data image. +uint8 sample_size # Size in bytes of each "pixel" in the ping data. + +int16[] bearings # Bearing angle of each column of the sonar data + # (in 100th of a degree, multiply by 0.01 to get a + # value in degrees). + # The sonar image is not sampled uniformly in the + # bearing direction. + +uint8[] ping_data # Ping data (is a row major image in little-endian). + # Size in bytes of each pixel is given in the + # sampleSize field. The size in bytes of each line + # is given in the step field. If the hasGains + # field is true, each row starts with 4 bytes + # containing gain of the row (encoded as a little + # endian uint32. Divide the whole row by the square + # root of this gain to have consistent value across + # the image data). diff --git a/oculus_interfaces/package.xml b/oculus_interfaces/package.xml index ce9cd91..fec1a7d 100644 --- a/oculus_interfaces/package.xml +++ b/oculus_interfaces/package.xml @@ -2,7 +2,7 @@ oculus_interfaces 0.0.1 - TODO: Package description + ROS 2 messages interfaces for BluePrint subsea Oculus M1200d sonar Auguste BOURGOIS BSD3 diff --git a/oculus_ros2/CMakeLists.txt b/oculus_ros2/CMakeLists.txt index 6f636cc..405f63e 100644 --- a/oculus_ros2/CMakeLists.txt +++ b/oculus_ros2/CMakeLists.txt @@ -1,50 +1,67 @@ cmake_minimum_required(VERSION 3.16) project(oculus_ros2 VERSION 1.0) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -ggdb3) +endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(oculus_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(rclpy REQUIRED) -find_package(std_msgs REQUIRED) find_package(rcl_interfaces REQUIRED) - -find_package(oculus_driver QUIET) -if(NOT TARGET oculus_driver) - include(FetchContent) - FetchContent_Declare(oculus_driver - GIT_REPOSITORY https://github.com/ENSTABretagneRobotics/oculus_driver.git - GIT_TAG v1.0 - ) - FetchContent_MakeAvailable(oculus_driver) -endif() +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(oculus_driver REQUIRED) +find_package(oculus_interfaces REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(OpenCV 4.5.4 REQUIRED) add_executable(oculus_sonar_node src/oculus_sonar_node.cpp + src/sonar_viewer.cpp ) -target_link_libraries(oculus_sonar_node PUBLIC - ${ament_LIBRARIES} +target_include_directories(oculus_sonar_node PUBLIC + $ + $ +) +target_link_libraries(oculus_sonar_node PRIVATE oculus_driver ) -target_compile_features(oculus_sonar_node PRIVATE cxx_std_17) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - target_compile_options(oculus_sonar_node PRIVATE -Wall -Wextra -Wpedantic) -endif() - ament_target_dependencies(oculus_sonar_node PUBLIC - rclcpp - oculus_interfaces - rcl_interfaces + rclcpp + oculus_interfaces + rcl_interfaces + sensor_msgs + OpenCV + cv_bridge ) -# INSTALL -install(PROGRAMS scripts/bag_to_oculus - DESTINATION bin) -install(DIRECTORY launch cfg DESTINATION share/${PROJECT_NAME}) +add_executable(oculus_viewer_node + src/oculus_viewer_node.cpp + src/sonar_viewer.cpp +) +target_include_directories(oculus_viewer_node PUBLIC + $ + $ +) +target_link_libraries(oculus_viewer_node PRIVATE + oculus_driver +) +ament_target_dependencies(oculus_viewer_node PUBLIC + rclcpp + oculus_interfaces + rcl_interfaces + sensor_msgs + OpenCV + cv_bridge +) -install(TARGETS - oculus_sonar_node - DESTINATION lib/${PROJECT_NAME}) +install(PROGRAMS scripts/display_oculus_file.py scripts/oculus_to_rosbag.py DESTINATION bin) +install(DIRECTORY launch cfg DESTINATION share/${PROJECT_NAME}) +install(PROGRAMS scripts/oculus_subscriber_to_image.py DESTINATION lib/${PROJECT_NAME}) +install(TARGETS oculus_sonar_node oculus_viewer_node DESTINATION lib/${PROJECT_NAME}) ament_package() - diff --git a/oculus_ros2/cfg/default.yaml b/oculus_ros2/cfg/default.yaml index 3be532b..ada27ce 100644 --- a/oculus_ros2/cfg/default.yaml +++ b/oculus_ros2/cfg/default.yaml @@ -1,12 +1,17 @@ oculus_sonar: ros__parameters: - frame_id: "sonar" # Frame of reference for the ping messages. + frame_id: "sonar" # Frame of reference for the ping messages. Default value is "sonar". - frequency_mode: 1 # Sonar beam frequency mode. + temperature_warn: 30. # Sonar temperature at which a warning is raised. Default value is 30.0. + temperature_stop: 35. # Sonar temperature at which the sonar goes to standby mode. Default value is 35.0. + + run: False # If run is False, stanby mode is forced. Default value is False. + + frequency_mode: 1 # Sonar beam frequency mode. Default value is 1. # 1: Low frequency (1.2MHz, wide aperture). # 2: High frequency (2.1MHz, narrow aperture). - ping_rate: 0 # Frequency of ping fires. + ping_rate: 2 # Frequency of ping fires. Default value is 2. # 0: 10Hz max ping rate. # 1: 15Hz max ping rate. # 2: 40Hz max ping rate. @@ -14,35 +19,18 @@ oculus_sonar: # 4: 2Hz max ping rate. # 5: Standby mode (no ping fire). - # # Hint on available network speed (no feedback, sonar seems to ignore it) - # network_speed: 0 # Hint on network available speed ? min=0, max=255 - - # Set to non-configurable (by choice) - # # the next 3 parameters are used to fill the OculusFireMessage.flags parameter - # # For now only compatible with SimplePingResult - # range_type: 1 # Range unit type. - # 0: Interpret range in percents. - # 1: Interpret range in meters. - - - data_depth: 0 # Ping data encoding bit count. - # 0: Ping data encoded on 8bits. - # 1: Ping data encoded on 16bits. - - nbeams: 0 # Number of ping beams. + nbeams: 1 # Number of ping beams. Default value is 1. # 0: Oculus outputs 256 beams. # 1: Oculus outputs 512 beams. - send_gain: false # Send range gain with data. - gain_assist: false # Enable auto gain. - # full_ping: false # Send full ping message. + gain_assist: True # Enable gain assist. Default value is True. - range: 3.0 # Sonar range (in meters), min=0.3, max=40.0. + range: 20.0 # Sonar range (in meters), min=0.3, max=40.0. Default value is 20.0. # These parameters are for scaling data (investigate) - gamma_correction: 127 # Gamma correction, min=0, max=255 - gain_percent: 50.0 # Gain percentage (%), min=0.1, max=100.0 + gamma_correction: 153 # Gamma correction, min=0, max=255. Default value is 153 (60%). + gain_percent: 50.0 # Gain percentage (%), min=0.1, max=100.0. Default value is 50.0. - sound_speed: 0.0 # Sound speed (in m/s, set to 0 for it to be calculated using salinity), min=1400.0, max=1600.0 - use_salinity: true # Use salinity to calculate sound_speed. - salinity: 0.0 # Salinity (in parts per thousand (ppt,ppm,g/kg), used to calculate sound speed if needed), min=0.0, max=100 + sound_speed: 0.0 # Sound speed (in m/s, set to 0 for it to be calculated using salinity), min=1400.0, max=1600.0. Default value is 0.0. + use_salinity: True # Use salinity to calculate sound_speed. Default value is True. + salinity: 0.0 # Salinity (in parts per thousand (ppt,ppm,g/kg), used to calculate sound speed if needed), min=0.0, max=100. Default value is 0.0. diff --git a/oculus_ros2/include/oculus_ros2/conversions.hpp b/oculus_ros2/include/oculus_ros2/conversions.hpp new file mode 100644 index 0000000..3b9f80f --- /dev/null +++ b/oculus_ros2/include/oculus_ros2/conversions.hpp @@ -0,0 +1,166 @@ +/** + * BSD 3-Clause License + * + * Copyright (c) 2022, ENSTA-Bretagne + * 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 the copyright holder 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 HOLDER 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. + */ + +#ifndef OCULUS_ROS2__CONVERSIONS_HPP_ +#define OCULUS_ROS2__CONVERSIONS_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +// #include + +namespace oculus { + +inline rclcpp::Time toMsg(const SonarDriver::TimePoint& stamp) { + size_t nano = std::chrono::duration_cast(stamp.time_since_epoch()).count(); + size_t seconds = nano / 1000000000; + return rclcpp::Time(seconds, nano - 1000000000 * seconds); +} + +inline void toMsg(oculus_interfaces::msg::OculusHeader& msg, const OculusMessageHeader& header) { + msg.oculus_id = header.oculusId; + msg.src_device_id = header.srcDeviceId; + msg.dst_device_id = header.dstDeviceId; + msg.msg_id = header.msgId; + msg.msg_version = header.msgVersion; + msg.payload_size = header.payloadSize; + msg.spare2 = header.spare2; +} + +inline void toMsg(oculus_interfaces::msg::OculusVersionInfo& msg, const OculusVersionInfo& version) { + msg.firmware_version0 = version.firmwareVersion0; + msg.firmware_date0 = version.firmwareDate0; + msg.firmware_version1 = version.firmwareVersion1; + msg.firmware_date1 = version.firmwareDate1; + msg.firmware_version2 = version.firmwareVersion2; + msg.firmware_date2 = version.firmwareDate2; +} + +inline void toMsg(oculus_interfaces::msg::OculusStatus& msg, const OculusStatusMsg& status) { + toMsg(msg.hdr, status.hdr); + + msg.device_id = status.deviceId; + msg.device_type = status.deviceType; + msg.part_number = status.partNumber; + msg.status = status.status; + + toMsg(msg.version_info, status.versinInfo); + + msg.ip_addr = status.ipAddr; + msg.ip_mask = status.ipMask; + msg.connected_ip_addr = status.connectedIpAddr; + + msg.mac_addr0 = status.macAddr0; + msg.mac_addr1 = status.macAddr1; + msg.mac_addr2 = status.macAddr2; + msg.mac_addr3 = status.macAddr3; + msg.mac_addr4 = status.macAddr4; + msg.mac_addr5 = status.macAddr5; + + msg.temperature0 = status.temperature0; + msg.temperature1 = status.temperature1; + msg.temperature2 = status.temperature2; + msg.temperature3 = status.temperature3; + msg.temperature4 = status.temperature4; + msg.temperature5 = status.temperature5; + msg.temperature6 = status.temperature6; + msg.temperature7 = status.temperature7; + msg.pressure = status.pressure; +} + +inline void toMsg(oculus_interfaces::msg::OculusFireConfig& msg, const OculusSimpleFireMessage& fireConfig) { + toMsg(msg.head, fireConfig.head); + + msg.master_mode = fireConfig.masterMode; + msg.ping_rate = fireConfig.pingRate; + msg.network_speed = fireConfig.networkSpeed; + msg.gamma_correction = fireConfig.gammaCorrection; + msg.flags = fireConfig.flags; + msg.range = fireConfig.range; + msg.gain_percent = fireConfig.gainPercent; + msg.speed_of_sound = fireConfig.speedOfSound; + msg.salinity = fireConfig.salinity; +} + +inline void toMsg(oculus_interfaces::msg::OculusPing& msg, const OculusSimplePingResult& ping) { + toMsg(msg.fire_message, ping.fireMessage); + msg.ping_id = ping.pingId; + msg.status = ping.status; + msg.frequency = ping.frequency; + msg.temperature = ping.temperature; + msg.pressure = ping.pressure; + msg.speeed_of_sound_used = ping.speeedOfSoundUsed; + msg.ping_start_time = ping.pingStartTime; + msg.data_size = ping.dataSize; + msg.range_resolution = ping.rangeResolution; + msg.n_ranges = ping.nRanges; + msg.n_beams = ping.nBeams; + msg.image_offset = ping.imageOffset; + msg.image_size = ping.imageSize; + msg.message_size = ping.messageSize; +} + +inline void toMsg(oculus_interfaces::msg::Ping& msg, const oculus::PingMessage::ConstPtr& ping) { + msg.header.stamp = toMsg(ping->timestamp()); + + msg.ping_id = ping->ping_index(); + msg.ping_firing_date = ping->ping_firing_date(); + msg.range = ping->range(); + msg.gain_percent = ping->gain_percent(); + msg.frequency = ping->frequency(); + msg.speed_of_sound_used = ping->speed_of_sound_used(); + msg.range_resolution = ping->range_resolution(); + msg.temperature = ping->temperature(); + msg.pressure = ping->pressure(); + msg.master_mode = ping->master_mode(); + msg.has_gains = ping->has_gains(); + msg.n_ranges = ping->range_count(); + msg.n_beams = ping->bearing_count(); + msg.step = ping->step(); + msg.sample_size = ping->sample_size(); + + msg.bearings.assign(ping->bearing_data(), ping->bearing_data() + ping->bearing_count()); + msg.ping_data = ping->data(); +} + +} // namespace oculus + +#endif // OCULUS_ROS2__CONVERSIONS_HPP_ diff --git a/oculus_ros2/include/oculus_ros2/oculus_sonar_node.hpp b/oculus_ros2/include/oculus_ros2/oculus_sonar_node.hpp new file mode 100644 index 0000000..b91f47f --- /dev/null +++ b/oculus_ros2/include/oculus_ros2/oculus_sonar_node.hpp @@ -0,0 +1,236 @@ +/** + * BSD 3-Clause License + * + * Copyright (c) 2022, ENSTA-Bretagne + * 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 the copyright holder 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 HOLDER 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. + */ + +#ifndef OCULUS_ROS2__OCULUS_SONAR_NODE_HPP_ +#define OCULUS_ROS2__OCULUS_SONAR_NODE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +struct SonarParameters { + int frequency_mode; + int ping_rate; + int nbeams; + bool gain_assist; + double range; + int gamma_correction; + double gain_percent; + double sound_speed; + bool use_salinity; + double salinity; +}; + +namespace flagByte { +const int RANGE_AS_METERS = 0x01; // bit 0: 0 = interpret range as percent, 1 = interpret range as meters +// const int ?? = 0x02; // bit 1: ?? const int DATA_DEPTH = 0x02; // bit 1: 0 = 8 bit data, 1 = 16 bit data // inverted ? +// TODO(hugoyvrn) +const int SEND_GAINS = 0x04; // bit 2: 0 = won't send gain, 1 = send gain +const int SIMPLE_PING = 0x08; // bit 3: 0 = send full return message, 1 = send simple return message +const int GAIN_ASSIST = 0x10; // bit 4: gain assist? +// const int ?? = 0x20; // bit 5: ? +const int NBEAMS = 0x40; // bit 6: enable 512 beams +// const int ?? = 0x80; // bit 7: ? +} // namespace flagByte + +namespace params { + +const double TEMPERATURE_WARN_DEFAULT_VALUE = 30.; +const double TEMPERATURE_STOP_DEFAULT_VALUE = 35.; +const bool RUN_MODE_DEFAULT_VALUE = false; + +struct BoolParam { + const std::string name; + const bool default_val; + const std::string desc; +}; + +const BoolParam GAIN_ASSIT = {"gain_assist", true, ""}; +const BoolParam USE_SALINITY = {"use_salinity", true, "Use salinity to calculate sound_speed."}; + +const std::vector BOOL = {GAIN_ASSIT, USE_SALINITY}; + +struct IntParam { + const std::string name; + const int min; + const int max; + const int default_val; + const std::string desc; +}; + +const IntParam FREQUENCY_MODE = {"frequency_mode", 1, 2, 2, + "Sonar beam frequency mode.\n" + "\t1: Low frequency (1.2MHz, wide aperture).\n" + "\t2: High frequency (2.1Mhz, narrow aperture)."}; + +const IntParam PING_RATE = {"ping_rate", 0, 5, 2, + "Frequency of ping fires.\n\t" + std::to_string(pingRateNormal) + ": 10Hz max ping rate.\n\t" + std::to_string(pingRateHigh) + + ": 15Hz max ping rate.\n\t" + std::to_string(pingRateHighest) + ": 40Hz max ping rate.\n\t" + + std::to_string(pingRateLow) + ": 5Hz max ping rate.\n\t" + std::to_string(pingRateLowest) + ": 2Hz max ping rate.\n\t" + + static_cast(pingRateStandby) + ": Standby mode (no ping fire)."}; +const IntParam NBEAMS = {"nbeams", 0, 1, 1, + "Number of ping beams.\n" + "\t0: Oculus outputs 256 beams.\n" + "\t1: Oculus outputs 512 beams."}; +const IntParam GAMMA_CORRECTION = {"gamma_correction", 0, 255, 153, "Gamma correction, min=0, max=255."}; + +const std::vector INT = {FREQUENCY_MODE, PING_RATE, NBEAMS, GAMMA_CORRECTION}; + +struct DoubleParam { + const std::string name; + const double min; + const double max; + const double step; + const double default_val; + const std::string desc; +}; + +const DoubleParam RANGE = {"range", .3, 40., .1, 20., "Sonar range (in meters), min=0.3, max=40.0."}; +const DoubleParam GAIN_PERCENT = {"gain_percent", .1, 100., .1, 50., "Gain percentage (%), min=0.1, max=100.0."}; +const DoubleParam SOUND_SPEED = {"sound_speed", 1400., 1600., .1, 1500., + "Sound speed (in m/s, set to 0 for it to be calculated using salinity), min=1400.0, max=1600.0."}; +const DoubleParam SALINITY = {"salinity", 0., 100., .1, 0., + "Salinity (in parts per thousand (ppt,ppm,g/kg), " + "used to calculate sound speed if needed), min=0.0, max=100"}; + +const std::vector DOUBLE = {RANGE, GAIN_PERCENT, SOUND_SPEED, SALINITY}; + +} // namespace params + +class OculusSonarNode : public rclcpp::Node { +public: + OculusSonarNode(); + ~OculusSonarNode(); + +protected: + const std::vector dynamic_parameters_names_{"frequency_mode", "ping_rate", "nbeams", "gain_assist", "range", + "gamma_correction", "gain_percent", "sound_speed", "use_salinity", "salinity", "run"}; + + SonarParameters currentSonarParameters_; + SonarParameters currentRosParameters_; + oculus::SonarDriver::PingConfig currentConfig_; + + bool is_running_; // State value. Same value as ros parameter "run" + bool is_overheating_ = false; // State value + + mutable std::shared_mutex param_mutex_; // multithreading protection + + int get_subscription_count() const; + +private: + std::shared_ptr sonar_driver_; + oculus::AsyncService io_service_; + // rclcpp::Publisher::SharedPtr image_publisher_; + SonarViewer sonar_viewer_; + const std::string frame_id_; + const double temperature_warn_limit_; + const double temperature_stop_limit_; + rclcpp::Publisher::SharedPtr status_publisher_{nullptr}; + rclcpp::Publisher::SharedPtr ping_publisher_{nullptr}; + rclcpp::Publisher::SharedPtr temperature_publisher_{nullptr}; + rclcpp::Publisher::SharedPtr pressure_publisher_{nullptr}; + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_{nullptr}; + + template + void updateRosConfigForParam(T& currentSonar_param, const T& new_param, const std::string& param_name); + void updateRosConfig(); + template + void handleFeedbackForParam(rcl_interfaces::msg::SetParametersResult& result, + const rclcpp::Parameter& param, + const T& old_val, + const T& new_val, + const std::string& param_name) const; + void updateLocalParameters(SonarParameters& parameters, const std::vector& new_parameters); + void updateLocalParameters(SonarParameters& parameters, oculus::SonarDriver::PingConfig feedback); + void sendParamToSonar(rclcpp::Parameter param, rcl_interfaces::msg::SetParametersResult result); + rcl_interfaces::msg::SetParametersResult setConfigCallback(const std::vector& parameters); + + void enableRunMode(); + void disableRunMode(); + void checkOverheating(const double& new_temperature); + void setMinimalFlags(uint8_t& flags) const; + void checkMinimalFlags(const uint8_t& flags) const; + void publishStatus(const OculusStatusMsg& status); + void publishPing(const oculus::PingMessage::ConstPtr& pingMetadata); + void handleDummy(); +}; + +template +void OculusSonarNode::updateRosConfigForParam(T& currentSonar_param, const T& new_param, const std::string& param_name) { + if (currentSonar_param != new_param) { + this->remove_on_set_parameters_callback(this->param_cb_.get()); + RCLCPP_WARN_STREAM(this->get_logger(), + "The parameter " << param_name << " has change by it self from " << currentSonar_param << " to " << new_param); + currentSonar_param = new_param; + this->set_parameter(rclcpp::Parameter(param_name, new_param)); + this->param_cb_ = + this->add_on_set_parameters_callback(std::bind(&OculusSonarNode::setConfigCallback, this, std::placeholders::_1)); + } +} + +template +void OculusSonarNode::handleFeedbackForParam(rcl_interfaces::msg::SetParametersResult& result, + const rclcpp::Parameter& param, + const T& old_val, + const T& new_val, + const std::string& param_name) const { + if (old_val != new_val) { + if (param.get_name() == param_name) { + result.successful = false; + RCLCPP_WARN_STREAM(this->get_logger(), "Could not update " << param_name); + result.reason.append("Could not update " + param_name + ".\n"); + } else { + RCLCPP_WARN_STREAM(this->get_logger(), + param_name << " change from " << old_val << " to " << new_val << " when updating the parameter " << param.get_name()); + result.reason.append(param_name + " change.\n"); + } + } +} + +#endif // OCULUS_ROS2__OCULUS_SONAR_NODE_HPP_ diff --git a/oculus_ros2/include/oculus_ros2/oculus_viewer_node.hpp b/oculus_ros2/include/oculus_ros2/oculus_viewer_node.hpp new file mode 100644 index 0000000..ad6d299 --- /dev/null +++ b/oculus_ros2/include/oculus_ros2/oculus_viewer_node.hpp @@ -0,0 +1,56 @@ +/** + * BSD 3-Clause License + * + * Copyright (c) 2022, ENSTA-Bretagne + * 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 the copyright holder 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 HOLDER 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. + */ + +#ifndef OCULUS_ROS2__OCULUS_VIEWER_NODE_HPP_ +#define OCULUS_ROS2__OCULUS_VIEWER_NODE_HPP_ + +#include + +#include + +#include +#include +#include + +class OculusViewerNode : public rclcpp::Node { +public: + OculusViewerNode(); + ~OculusViewerNode(); + +private: + // rclcpp::Publisher::SharedPtr image_publisher_; + SonarViewer sonar_viewer_; + rclcpp::Subscription::SharedPtr ping_subscription_; + void pingCallback(const oculus_interfaces::msg::Ping& ping_msg) const; +}; + +#endif // OCULUS_ROS2__OCULUS_VIEWER_NODE_HPP_ diff --git a/oculus_ros2/include/oculus_ros2/sonar_viewer.hpp b/oculus_ros2/include/oculus_ros2/sonar_viewer.hpp new file mode 100644 index 0000000..e9c7d20 --- /dev/null +++ b/oculus_ros2/include/oculus_ros2/sonar_viewer.hpp @@ -0,0 +1,81 @@ +/** + * BSD 3-Clause License + * + * Copyright (c) 2022, ENSTA-Bretagne + * 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 the copyright holder 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 HOLDER 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. + */ + +#ifndef OCULUS_ROS2__SONAR_VIEWER_HPP_ +#define OCULUS_ROS2__SONAR_VIEWER_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +class SonarViewer { +public: + explicit SonarViewer(rclcpp::Node* node); + ~SonarViewer(); + void publishFan(const oculus::PingMessage::ConstPtr& ping, const std::string& frame_id = "sonar") const; + void publishFan(const oculus_interfaces::msg::Ping& ros_ping_msg) const; + void publishFan(const int& width, + const int& height, + const int& offset, + const std::vector& ping_data, + const int& master_mode, + const std_msgs::msg::Header& header) const; + + rclcpp::Publisher::SharedPtr image_publisher_; + +protected: + const double LOW_FREQUENCY_BEARING_APERTURE_ = 65.; + const double HIGHT_FREQUENCY_BEARING_APERTURE_ = 40.; + const int SIZE_OF_GAIN_ = 4; + +private: + const rclcpp::Node* node_; +}; + +#endif // OCULUS_ROS2__SONAR_VIEWER_HPP_ diff --git a/oculus_ros2/launch/default.launch.py b/oculus_ros2/launch/default.launch.py index 04299cb..b070c7b 100644 --- a/oculus_ros2/launch/default.launch.py +++ b/oculus_ros2/launch/default.launch.py @@ -1,39 +1,63 @@ +# BSD 3-Clause License +# +# Copyright (c) 2022, ENSTA-Bretagne +# 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 the copyright holder 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 HOLDER 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. + import os from ament_index_python.packages import get_package_share_directory -from launch.actions import DeclareLaunchArgument from launch import LaunchDescription from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration def generate_launch_description(): - + ld = LaunchDescription() - - ld.add_action(DeclareLaunchArgument( - name='port', - default_value='this_is_a_port', - description='Filters Configuration')) - + config = os.path.join( - get_package_share_directory('oculus_ros2'), - 'cfg', - 'default.yaml' - ) - + get_package_share_directory("oculus_ros2"), "cfg", "default.yaml" + ) + oculus_sonar_node = Node( - package='oculus_ros2', - executable='oculus_sonar_node', - name='oculus_sonar', - # parameters=[config], - remappings=[ - ('ping', '/oculus_sonar/ping'), # Topic name where ping messages are published (cf Oculus.h). - ('status', '/oculus_sonar/status') # Topic name where status messages are published (cf Oculus.h). - ], - arguments=['-port', LaunchConfiguration('port')], - output='screen' - ) + package="oculus_ros2", + executable="oculus_sonar_node", + name="oculus_sonar", + parameters=[config], + namespace="sonar", + remappings=[ + ("status", "status"), + ("ping", "ping"), + ("temperature", "temperature"), + ("pressure", "pressure"), + ], + output="screen", + ) + ld.add_action(oculus_sonar_node) return ld diff --git a/oculus_ros2/launch/pings_only.launch.py b/oculus_ros2/launch/pings_only.launch.py new file mode 100644 index 0000000..6cc2424 --- /dev/null +++ b/oculus_ros2/launch/pings_only.launch.py @@ -0,0 +1,76 @@ +# BSD 3-Clause License +# +# Copyright (c) 2022, ENSTA-Bretagne +# 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 the copyright holder 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 HOLDER 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. +import os + +from ament_index_python.packages import get_package_share_directory +from launch.actions import DeclareLaunchArgument +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + + ld = LaunchDescription() + + ld.add_action( + DeclareLaunchArgument( + name="port", + default_value="this_is_a_port", + description="Filters Configuration", + ) + ) + + config = os.path.join( + get_package_share_directory("oculus_ros2"), "cfg", "default.yaml" + ) + + oculus_sonar_node = Node( + package="oculus_ros2", + executable="oculus_sonar_node", + name="oculus_sonar", + # parameters=[config], + remappings=[ + ( + "ping", + "/oculus_sonar/ping", + ), # Topic name where ping messages are published (cf Oculus.h). + ( + "status", + "/oculus_sonar/status", + ), # Topic name where status messages are published (cf Oculus.h). + ], + arguments=["-port", LaunchConfiguration("port")], + output="screen", + ) + + ld.add_action(oculus_sonar_node) + + return ld diff --git a/oculus_ros2/package.xml b/oculus_ros2/package.xml index 9ad31fa..6281111 100644 --- a/oculus_ros2/package.xml +++ b/oculus_ros2/package.xml @@ -2,32 +2,30 @@ oculus_ros2 0.0.1 - ROS node for BluePrint subsea Oculus sonar + ROS 2 nodes for BluePrint subsea Oculus M1200d sonar Auguste BOURGOIS BSD3 - - ament_cmake - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - message_generation - message_runtime - - Boost - oculus_interfaces - rcl_interfaces + + ament_cmake + rclcpp + rclpy + rcl_interfaces + std_msgs + sensor_msgs + oculus_driver + oculus_interfaces + cv_bridge + OpenCV + + + ament_lint_auto ament_lint_common + ament_cmake diff --git a/oculus_ros2/scripts/bag_to_oculus b/oculus_ros2/scripts/bag_to_oculus deleted file mode 100755 index 04c9d04..0000000 --- a/oculus_ros2/scripts/bag_to_oculus +++ /dev/null @@ -1,127 +0,0 @@ -#! /usr/bin/python3 - -import os -import sys -import pathlib -import struct -import argparse - -import rosbag - -parser = argparse.ArgumentParser(description='Extract an Oculus sonar ping messages from a ROS bag and save them in the .oculus native format.') -parser.add_argument('input', type=str, help='Path to rosbag.') -parser.add_argument('-o', dest='output', type=str, help='Path where to save the .oculus file.') -parser.add_argument('-t', dest='topic', type=str, default='', - help='Name of the topic containing the ping.') -parser.add_argument('-m', dest='msg_type', type=str, default='oculus_sonar/OculusPing', - help='ROS message type of the ping (change this only if you know what you are doing).') -parser.add_argument('-U', '--use-input-dir', dest='use_input_dir', default=False, action='store_true', - help='Use input dir and auto-generated filename as output path') -args = parser.parse_args() - -# Checking input path -if not os.path.isfile(args.input): - raise ValueError('Invalid input, '+args.input+' : is not a regular file.') - -# if using input dir, generating a path -if args.use_input_dir: - args.output = os.path.split(args.input)[0] -# Checking output path -if args.output: - # if output provided, checking if is directory - if os.path.isdir(args.output): - # if is directory, appending generated filename - args.output = os.path.join(args.output, - os.path.splitext(os.path.split(args.input)[1])[0] + '.oculus') -else: - # If no output provided, generating one - args.output = os.path.splitext(os.path.split(args.input)[1])[0] + '.oculus' -# Checking if output file exists -# if os.path.isfile(args.output): -if os.path.isfile(args.output): - confirmation = input("Output file '" + args.output - +"' already exists. Overwrite ? [y/n]") - if confirmation[0] != 'y' and confirmation[0] != 'Y': - print('Aborting.') - sys.exit() - -bag = rosbag.Bag(args.input, 'r') -infos = bag.get_type_and_topic_info() - -# Checking rosbag for valid input -if not args.topic: - # no user defined topic, selecting one automatically with message type - # oculus_sonar/OculusPing. - - # Checking if the rosbag effectively contains some ping data. - if args.msg_type not in infos.msg_types.keys(): - raise ValueError("ROS bag '" + args.input - +"' does not seem to contain any '"+args.msg_type+" messages.") - - # Finding topic names which type is oculus_sonar/OculusPing - topicNames = [name for name,info in infos.topics.items() - if info.msg_type == args.msg_type] - if len(topicNames) > 1: - raise ValueError("The ROS bag '"+args.input - + "' contains more than one ping topic : " - + str(topicNames) - + ". Please select one with the '-t' option." ) - if len(topicNames) == 0: - raise ValueError("The ROS bag '"+args.input+"' knows the '" - + args.msg_type+ "' but doesn't contain any topic " - + "with this type. It may be corrupted.") - - # Only one topic using the oculus ping message type - args.topic = topicNames[0] -if infos.topics[args.topic].message_count < 1: - raise ValueError("No message in '"+args.topic+"' topic.") - -print('Starting ping extraction') -print('input :', args.input) -print('output :', args.output) -print('topic :', args.topic) -print('message type :', args.msg_type) - -with open(args.output,'wb') as outputFile: - log_header_struct = ( - {'name': 'fileHeader', 'type': 'I','val' : 2037}, - {'name': 'sizeHeader', 'type': 'I','val' : 48}, - {'name': 'source', 'type': '16s','val' : "Oculus".encode('ascii')}, - {'name': 'version', 'type': 'H','val' : 1}, - {'name': 'encryption', 'type': 'H','val' : 0}, - {'name': 'key', 'type': 'Q','val' : 0}, - {'name': 'spare1', 'type': 'I','val' : 123}, - {'name': 'time_unix', 'type': 'd','val' : 0}, - ) - - log_header_fmt =list(map(lambda x: x['type'], log_header_struct)) - log_header_names = list(map(lambda x: x['name'], log_header_struct)) - log_header_val = list(map(lambda x: x['val'], log_header_struct)) - - for ii in range(len(log_header_val)): - outputFile.write(struct.pack(log_header_fmt[ii], log_header_val[ii])) - - - for i,bagMessage in enumerate(bag.read_messages(topics=[args.topic])): - header_struct = ( - {'name': 'itemHeader', 'type': 'I','val' : 2037}, - {'name': 'sizeHeader', 'type': 'I','val' : 40}, - {'name': 'messageType', 'type': 'H','val' : 10}, - {'name': 'version', 'type': 'H','val' : 2}, - {'name': 'spare1', 'type': 'I','val' : 125}, - {'name': 'time_unix', 'type': 'd','val' : bagMessage.timestamp.to_time()}, - {'name': 'compression', 'type': 'H','val' : 0}, - {'name': 'spare2', 'type': 'H','val' : 126}, - {'name': 'rawSize', 'type': 'I','val' : 0}, - {'name': 'payloadSize', 'type': 'I','val' : 0}, - ) - - header_fmt =list(map(lambda x: x['type'], header_struct)) - header_names = list(map(lambda x: x['name'], header_struct)) - header_val = list(map(lambda x: x['val'], header_struct)) - - for ii in range(len(header_val)): - outputFile.write(struct.pack(header_fmt[ii], header_val[ii])) - - outputFile.write(bagMessage.message.data) -print("Done.") diff --git a/oculus_ros2/scripts/display_oculus_file.py b/oculus_ros2/scripts/display_oculus_file.py new file mode 100755 index 0000000..3fd56ca --- /dev/null +++ b/oculus_ros2/scripts/display_oculus_file.py @@ -0,0 +1,216 @@ +#! /usr/bin/python3 + +# BSD 3-Clause License +# +# Copyright (c) 2022, ENSTA-Bretagne +# 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 the copyright holder 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 HOLDER 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. + +import numpy as np +import matplotlib.pyplot as plt +import argparse +import time + +from oculus_python.files import OculusFileReader + + +def display_oculus_ping(msg, ax, args): + if msg is None: + print("File seems to be empty. Aborting.") + # plt.close('all') + # print(msg.metadata()) + + # print("timestamp", msg.timestamp()) + # print("timestamp_micros", msg.timestamp_micros()) + # print("ping_index", msg.ping_index()) + # print("range", msg.range()) + # print("gain_percent", msg.gain_percent()) + # print("frequency", msg.frequency()) + # print("speed_of_sound_used", msg.speed_of_sound_used()) + # print("range_resolution", msg.range_resolution()) + # print("temperature", msg.temperature()) + # print("pressure", msg.pressure()) + + bearings = 0.01 * np.array(msg.bearing_data()) + linearAngles = np.linspace(bearings[0], bearings[-1], len(bearings)) + rawPingData = np.array(msg.raw_ping_data()) + gains = np.ones( + [ + msg.range_count(), + ], + dtype=np.float32, + ) + if msg.has_gains(): + gains = np.array(msg.gains()) + pingData = np.array(msg.ping_data()) / np.sqrt(gains)[:, np.newaxis] + # print("pingData =", list(pingData)) + # print('has gains :', msg.has_gains()) + # print('sample size :', msg.sample_size()) + # print('ping shape :', pingData.shape) + + # _, ax = plt.subplots(1,1,) + # ax.plot(bearings, '-o', label='bearings') + # ax.plot(linearAngles, '-o', label='linear bearings') + # ax.grid() + # ax.legend() + # ax.set_xlabel('bearing index') + # ax.set_ylabel('bearing angle') + + # _, ax = plt.subplots(1,1) + # ax.plot(gains, '-o', label='gains') + # ax.grid() + # ax.legend() + # ax.set_xlabel('range index') + # ax.set_ylabel('range gain') + + ax[0].imshow(rawPingData) + ax[0].set_ylabel("Range index") + ax[0].set_xlabel("Bearing index") + ax[0].set_title("Raw ping data") + + ax[1].imshow(pingData) + ax[1].set_xlabel("Bearing index") + ax[1].set_title("Ping data rescaled with gains") + + plt.pause(args.rate) + + +def main(): + + print("Opening", args.filename) + file = OculusFileReader(args.filename) + # this can be called several time to iterate through the pings + oculus_ping_msg = file.read_next_ping() + # will return None when finished + if oculus_ping_msg is None: + print("[oculus_to_bag] File seems to be empty. Aborting.") + return + # print("type(oculus_ping_msg) =", type(oculus_ping_msg)) + + k = 0 + start_time = time.perf_counter() + _, ax = plt.subplots(1, 2) + while oculus_ping_msg: + k += 1 + if not k % 20: + # print(k) + elapsed_time = time.perf_counter() - start_time + print( + "[oculus_to_bag] {} pings have been red in {:.2f} seconds.".format( + k, elapsed_time + ), + end="\r", + ) + + display_oculus_ping(oculus_ping_msg, ax, args) + + oculus_ping_msg = file.read_next_ping() + print("") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + prog="OculusFileReader", + description="Example of how to read and display the content of a .oculus " + + "file. This will display the first ping from a the file.", + ) + parser.add_argument("filename", type=str, help="Path to a .oculus file to display") + parser.add_argument( + "-r", "--rate", type=float, default=1e-2, help=". Default to 1e-2" + ) + + args = parser.parse_args() + + main() + + # parser = argparse.ArgumentParser( + # prog='OculusFileReader', + # description='Example of how to read and display the content of a .oculus ' + + # 'file. This will display the first ping from a the file.') + # parser.add_argument('filename', type=str, + # help='Path to a .oculus file to display') + # args = parser.parse_args() + + # print('Opening', args.filename) + + # f = OculusFileReader(args.filename) + + # msg = f.read_next_ping() # this can be called several time to iterate through the pings + # # will return None when finished + # if msg is None: + # print('File seems to be empty. Aborting.') + # print(msg.metadata()) + + # print("timestamp", msg.timestamp()) + # print("timestamp_micros", msg.timestamp_micros()) + # print("ping_index", msg.ping_index()) + # print("range", msg.range()) + # print("gain_percent", msg.gain_percent()) + # print("frequency", msg.frequency()) + # print("speed_of_sound_used", msg.speed_of_sound_used()) + # print("range_resolution", msg.range_resolution()) + # print("temperature", msg.temperature()) + # print("pressure", msg.pressure()) + + # bearings = 0.01*np.array(msg.bearing_data()) + # linearAngles = np.linspace(bearings[0], bearings[-1], len(bearings)) + # rawPingData = np.array(msg.raw_ping_data()) + # gains = np.ones([msg.range_count(),], dtype=np.float32) + # if msg.has_gains(): + # gains = np.array(msg.gains()) + # pingData = np.array(msg.ping_data()) / np.sqrt(gains)[:,np.newaxis] + + # print('has gains :', msg.has_gains()) + # print('sample size :', msg.sample_size()) + # print('ping shape :', pingData.shape) + + # _, ax = plt.subplots(1,1) + # ax.plot(bearings, '-o', label='bearings') + # ax.plot(linearAngles, '-o', label='linear bearings') + # ax.grid() + # ax.legend() + # ax.set_xlabel('bearing index') + # ax.set_ylabel('bearing angle') + + # _, ax = plt.subplots(1,1) + # ax.plot(gains, '-o', label='gains') + # ax.grid() + # ax.legend() + # ax.set_xlabel('range index') + # ax.set_ylabel('range gain') + + # _, ax = plt.subplots(1,2) + # ax[0].imshow(rawPingData) + # ax[0].set_ylabel('Range index') + # ax[0].set_xlabel('Bearing index') + # ax[0].set_title('Raw ping data') + + # ax[1].imshow(pingData) + # ax[1].set_xlabel('Bearing index') + # ax[1].set_title('Ping data rescaled with gains') + + # plt.show() diff --git a/oculus_ros2/scripts/oculus_subscriber_to_image.py b/oculus_ros2/scripts/oculus_subscriber_to_image.py new file mode 100755 index 0000000..ab2e1dc --- /dev/null +++ b/oculus_ros2/scripts/oculus_subscriber_to_image.py @@ -0,0 +1,157 @@ +#! /usr/bin/python3 + +# BSD 3-Clause License +# +# Copyright (c) 2022, ENSTA-Bretagne +# 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 the copyright holder 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 HOLDER 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. +import oculus_python + +from rcl_interfaces.msg import ParameterDescriptor + + +from oculus_interfaces.msg import Ping +from sensor_msgs.msg import Image +import rclpy +from rclpy.node import Node + +import numpy as np + +import datetime + + +class OculusDisplayer(Node): + def __init__(self): + super().__init__("oculus_subscriber_to_image") + + self.image_subscriber = self.create_subscription( + Ping, "sonar/ping", self.callback, 10 + ) + self.image_publisher = self.create_publisher(Image, "oculus_sonar/image", 10) + + self.declare_parameter( + name="freq", + value="0.", + descriptor=ParameterDescriptor( + name="TODO(hugoyvrn)", + description="TODO(hugoyvrn)", + additional_constraints="freq>=0", + ), + ) + self.freq = self.get_parameter("freq").get_parameter_value().double_value + + if self.freq > 0: + self.timer = self.create_timer(1 / self.freq, self.timer_callback) + self.msg_is_new = False + + def callback(self, oculus_ros_msg): + timestamp = datetime.datetime.fromtimestamp( + oculus_ros_msg.header.stamp.sec + oculus_ros_msg.header.stamp.nanosec * 1e-9 + ) + oculus_driver_msg = oculus_python.ping_message_from_bytes( + bytes(oculus_ros_msg.ping_data), timestamp + ) + + # meta = oculus_driver_msg.metadata() + + # bearings = 0.01*np.array(oculus_driver_msg.bearing_data()) + + # linearAngles = np.linspace(bearings[0], bearings[-1], len(bearings)) + # rawPingData = np.array(oculus_driver_msg.raw_ping_data()) + gains = np.ones( + [ + oculus_driver_msg.range_count(), + ], + dtype=np.float32, + ) + if oculus_driver_msg.has_gains(): + gains = np.array(oculus_driver_msg.gains()) + else: + self.get_logger().warn("Ping don't send gains.") + pingData = ( + np.array(oculus_driver_msg.ping_data()) / np.sqrt(gains)[:, np.newaxis] + ) + pingData = np.array(255 * pingData, dtype=np.uint8) + pingData.astype(np.uint8) + + assert pingData.shape == (oculus_ros_msg.n_ranges, oculus_ros_msg.n_beams) + + image_msg = Image() + image_msg.header.frame_id = "sonar" # TODO(hugoyvrn) + image_msg.header = oculus_ros_msg.header + image_msg.height = oculus_ros_msg.n_ranges + image_msg.width = oculus_ros_msg.n_beams + image_msg.encoding = "mono8" # or 'mono16' TODO(hugoyvrn) + image_msg.is_bigendian = False # default value TODO(hugoyvrn) + image_msg.step = oculus_ros_msg.n_beams + + image_msg.data = pingData.flatten().tobytes() + + if ( + not (image_msg.height == oculus_ros_msg.n_ranges) + or not (image_msg.step == image_msg.width == oculus_ros_msg.n_beams) + or not ( + len(image_msg.data) + == image_msg.height * image_msg.width + == oculus_ros_msg.n_ranges * oculus_ros_msg.n_beams + ) + ): + # self.get_logger().info("pingData = %s" % pingData) + self.get_logger().info( + "oculus_ros_msg.n_ranges = %d" % oculus_ros_msg.n_ranges + ) + self.get_logger().info( + "oculus_ros_msg.n_beams = %d" % oculus_ros_msg.n_beams + ) + self.get_logger().info("pingData.shape = (%d; %d)" % pingData.shape) + self.get_logger().warn("Issue on ping sonar shapes.") + + if self.freq <= 0: + self.image_publisher.publish(image_msg) + else: + self.msg = image_msg + self.msg_is_new == True + + def timer_callback(self): + if self.msg_is_new: + self.image_publisher.publish(self.msg) + self.msg_is_new == False + + +def main(args=None): + rclpy.init(args=args) + displayer_node = OculusDisplayer() + + rclpy.spin(displayer_node) + + displayer_node.destroy + + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/oculus_ros2/scripts/oculus_to_rosbag.py b/oculus_ros2/scripts/oculus_to_rosbag.py new file mode 100755 index 0000000..e26bfa0 --- /dev/null +++ b/oculus_ros2/scripts/oculus_to_rosbag.py @@ -0,0 +1,251 @@ +#! /usr/bin/python3 + +# BSD 3-Clause License +# +# Copyright (c) 2022, ENSTA-Bretagne +# 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 the copyright holder 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 HOLDER 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. + +from rclpy.time import Time +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +import rosbag2_py +from oculus_interfaces.msg import Ping +from rclpy.serialization import serialize_message +from rclpy.clock import Clock +import time + + +import matplotlib.pyplot as plt +import argparse +from oculus_python.files import OculusFileReader + + +class RosBagCreator: + """ + RosBagCreator allow to create rosbags. + """ + + def __init__(self, output_pass="bag_test"): + """ + Create a write for rosbag writing. + + Args: + output_pass (str, optional): Name of the output rosbag. Defaults to 'bag_test'. + """ + + self.writer = rosbag2_py.SequentialWriter() + + storage_options = rosbag2_py._storage.StorageOptions( + uri=output_pass, storage_id="sqlite3" + ) + converter_options = rosbag2_py._storage.ConverterOptions("", "") + self.writer.open(storage_options, converter_options) + + def new_topic(self, name, type, serialization_format="cdr"): + """ + Initialize a new topic. + + Args: + name (str): Name of the topic + type (str): Ros type of the topic + serialization_format (str, optional): _description_. Defaults to 'cdr'. + """ + topic_info = rosbag2_py._storage.TopicMetadata( + name=name, type=type, serialization_format=serialization_format + ) + self.writer.create_topic(topic_info) + + def custom_clock(self, nanoseconds=None): + """ + Return a ros time (rclpy.time.Time) with a custom time. + Clock().now() or custom_clock(nanoseconds=None) return a ros + time (rclpy.time.Time) with the current time while running the + script. + + Args: + nanoseconds (number, optional): Set your custom time in nanoseconds. Defaults to None (current time while rening the code). + + Returns: + rclpy.time.Time: rclpy.time.Time with custom time set to nanoseconds + """ + if nanoseconds == None: + return Clock().now() + ClockType = _rclpy.ClockType + clock_type = ClockType.SYSTEM_TIME + __clock = _rclpy.Clock(clock_type) + with Clock().handle: + rcl_time = __clock.get_now() + return Time(nanoseconds=nanoseconds, clock_type=Clock().clock_type) + + def publish(self, topic_name, msg, nanoseconds=None): + """ + Write to the rosbag a new published entry. + + Args: + topic_name (str): Name of the topic to publish + ping_msg (Any ros message): Ros message to publish + time_stamp (number, optional): Time stamp of the message. Defaults to None (current time while rening the code). + """ + self.writer.write( + topic_name, + serialize_message(msg), + self.custom_clock(nanoseconds).nanoseconds, + ) + + +class Oculus_parser(RosBagCreator): + def __init__(self): + parser = argparse.ArgumentParser( + prog="oculus_to_bag", description=".oculus file parser to rosbag." + ) + parser.add_argument( + "filename", type=str, help="Path to a .oculus file to display" + ) + parser.add_argument( + "destination", type=str, help="Path to the folder to save the rosbag." + ) + parser.add_argument( + "--secondsoffset", + type=float, + default=0, + help="Set a time offset in seconds. Useful to handle time lignes to utc. A secondsoffset of 1 will write the rosbag 1s later that written in the .oculus file. Default to 0", + ) + # TODO(hugoyvrn) + parser.add_argument( + "-st", + "--startingtime", + type=float, + default=0, + help="Nombre of seconds since the Epoch for the rosbag startingtime. No data timestamped before startingtime will be taken into account. Default to None.", + ) + # TODO(hugoyvrn) + parser.add_argument( + "-et", + "--endingtime", + type=float, + default=0, + help="Nombre of seconds since the Epoch for the rosbag endingtime. No data timestamped after endingtime will be taken into account. Default to None.", + ) + parser.add_argument( + "--topicname", + type=str, + default="/sonar/oculus", + help="Set the topic name for oculus message. Default to '/sonar/oculus'", + ) + self.args = parser.parse_args() + self.output_pass = ( + self.args.destination + "/" + self.args.filename.split("/")[-1][:-7] + ) + self.output_pass.replace("//", "/") + self.output_pass.replace("/./", "/") + super().__init__(self.output_pass) + + print("[oculus_to_bag] Opening", self.args.filename) + + self.new_topic(name=self.args.topicname, type="oculus_interfaces/msg/Ping") + + def create_ros_oculus_msg(self, ping_msg): + ros_msg = Ping() + + seconds = ping_msg.timestamp_micros() * 1e-6 + self.args.secondsoffset + ros_msg.header.stamp.sec = int(seconds) + ros_msg.header.stamp.nanosec = int((seconds % 1) * 1e9) + + ros_msg.header.frame_id = "sonar" + + ros_msg.range = ping_msg.range_resolution() + ros_msg.gain_percent = ping_msg.gain_percent() + ros_msg.frequency = ping_msg.frequency() + ros_msg.speed_of_sound_used = ping_msg.speed_of_sound_used() + ros_msg.range_resolution = ping_msg.range_resolution() + ros_msg.temperature = ping_msg.temperature() + ros_msg.pressure = ping_msg.pressure() + ros_msg.master_mode = ping_msg.master_mode() + ros_msg.has_gains = ping_msg.has_gains() + ros_msg.n_ranges = ping_msg.range_count() + ros_msg.n_beams = ping_msg.bearing_count() + ros_msg.step = ( + ping_msg.bearing_count() * ping_msg.sample_size() + 4 * ping_msg.has_gains() + ) + ros_msg.sample_size = ping_msg.sample_size() + ros_msg.bearings = ping_msg.bearing_data() + ros_msg.ping_data = ping_msg.message().data().tolist() + + # print(">>>>>>>>>>><", ros_msg, "\n\n") + + return ros_msg + + def parse(self): + + print( + "[oculus_to_bag] Parsing {} to {}. This may take few seconds.".format( + self.args.filename, self.output_pass + ) + ) + + self.file = OculusFileReader(self.args.filename) + # this can be called several time to iterate through the pings + ping_msg = self.file.read_next_ping() + # will return None when finished + if ping_msg is None: + print("[oculus_to_bag] File seems to be empty. Aborting.") + return + + k = 0 + start_time = time.perf_counter() + + while ping_msg: + k += 1 + # print(k) + elapsed_time = time.perf_counter() - start_time + print( + "[oculus_to_bag] {} pings have been parsed in {:.2f} seconds.".format( + k, elapsed_time + ), + end="\r", + ) + + ros_msg = self.create_ros_oculus_msg(ping_msg) + + self.publish( + topic_name=self.args.topicname, + msg=ros_msg, + nanoseconds=ping_msg.timestamp_micros() * 1e3 + + self.args.secondsoffset * 1e9, + ) + + # this can be called several time to iterate through the pings + ping_msg = self.file.read_next_ping() + # will return None when finished + + print("\n") + print("[oculus_to_bag] File parsing is done") + + +if __name__ == "__main__": + parseur = Oculus_parser() + parseur.parse() diff --git a/oculus_ros2/src/conversions.h b/oculus_ros2/src/conversions.h deleted file mode 100644 index 060c16f..0000000 --- a/oculus_ros2/src/conversions.h +++ /dev/null @@ -1,103 +0,0 @@ -#ifndef _DEF_OCULUS_ROS_CONVERSIONS_H_ -#define _DEF_OCULUS_ROS_CONVERSIONS_H_ - -#include -#include "oculus_interfaces/msg/oculus_header.hpp" -#include "oculus_interfaces/msg/oculus_version_info.hpp" -#include "oculus_interfaces/msg/oculus_status.hpp" -#include "oculus_interfaces/msg/oculus_fire_config.hpp" -#include "oculus_interfaces/msg/oculus_ping.hpp" - -namespace oculus { - -inline void copy_to_ros(oculus_interfaces::msg::OculusHeader &msg, const OculusMessageHeader& header) -{ - msg.oculus_id = header.oculusId; - msg.src_device_id = header.srcDeviceId; - msg.dst_device_id = header.dstDeviceId; - msg.msg_id = header.msgId; - msg.msg_version = header.msgVersion; - msg.payload_size = header.payloadSize; - msg.spare2 = header.spare2; -} - -inline void copy_to_ros(oculus_interfaces::msg::OculusVersionInfo &msg, const OculusVersionInfo& version) -{ - msg.firmware_version0 = version.firmwareVersion0; - msg.firmware_date0 = version.firmwareDate0; - msg.firmware_version1 = version.firmwareVersion1; - msg.firmware_date1 = version.firmwareDate1; - msg.firmware_version2 = version.firmwareVersion2; - msg.firmware_date2 = version.firmwareDate2; -} - -inline void copy_to_ros(oculus_interfaces::msg::OculusStatus &msg, const OculusStatusMsg& status) -{ - copy_to_ros(msg.hdr, status.hdr); - - msg.device_id = status.deviceId; - msg.device_type = status.deviceType; - msg.part_number = status.partNumber; - msg.status = status.status; - - copy_to_ros(msg.versin_info,status.versinInfo); - - msg.ip_addr = status.ipAddr; - msg.ip_mask = status.ipMask; - msg.connected_ip_addr = status.connectedIpAddr; - - msg.mac_addr0 = status.macAddr0; - msg.mac_addr1 = status.macAddr1; - msg.mac_addr2 = status.macAddr2; - msg.mac_addr3 = status.macAddr3; - msg.mac_addr4 = status.macAddr4; - msg.mac_addr5 = status.macAddr5; - - msg.temperature0 = status.temperature0; - msg.temperature1 = status.temperature1; - msg.temperature2 = status.temperature2; - msg.temperature3 = status.temperature3; - msg.temperature4 = status.temperature4; - msg.temperature5 = status.temperature5; - msg.temperature6 = status.temperature6; - msg.temperature7 = status.temperature7; - msg.pressure = status.pressure; -} - -inline void copy_to_ros(oculus_interfaces::msg::OculusFireConfig &msg, const OculusSimpleFireMessage& fireConfig) -{ - copy_to_ros(msg.head, fireConfig.head); - - msg.master_mode = fireConfig.masterMode; - msg.ping_rate = fireConfig.pingRate; - msg.network_speed = fireConfig.networkSpeed; - msg.gamma_correction = fireConfig.gammaCorrection; - msg.flags = fireConfig.flags; - msg.range = fireConfig.range; - msg.gain_percent = fireConfig.gainPercent; - msg.speed_of_sound = fireConfig.speedOfSound; - msg.salinity = fireConfig.salinity; -} - -inline void copy_to_ros(oculus_interfaces::msg::OculusPing &msg, const OculusSimplePingResult& ping) -{ - copy_to_ros(msg.fire_message, ping.fireMessage); - msg.ping_id = ping.pingId; - msg.status = ping.status; - msg.frequency = ping.frequency; - msg.temperature = ping.temperature; - msg.pressure = ping.pressure; - msg.speeed_of_sound_used = ping.speeedOfSoundUsed; - msg.ping_start_time = ping.pingStartTime; - msg.data_size = ping.dataSize; - msg.range_resolution = ping.rangeResolution; - msg.n_ranges = ping.nRanges; - msg.n_beams = ping.nBeams; - msg.image_offset = ping.imageOffset; - msg.image_size = ping.imageSize; - msg.message_size = ping.messageSize; -} - -} //namespace oculus - -#endif //_DEF_OCULUS_ROS_CONVERSIONS_H_ diff --git a/oculus_ros2/src/oculus_sonar_node.cpp b/oculus_ros2/src/oculus_sonar_node.cpp index fa92638..cade9eb 100644 --- a/oculus_ros2/src/oculus_sonar_node.cpp +++ b/oculus_ros2/src/oculus_sonar_node.cpp @@ -1,370 +1,498 @@ -using namespace std; +/** + * BSD 3-Clause License + * + * Copyright (c) 2022, ENSTA-Bretagne + * 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 the copyright holder 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 HOLDER 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. + */ #define BOOST_BIND_GLOBAL_PLACEHOLDERS -#include "oculus_sonar_node.hpp" +#include using SonarDriver = oculus::SonarDriver; -OculusSonarNode::OculusSonarNode() : Node("oculus_sonar") -{ - if (!this->has_parameter("frame_id")) { - this->declare_parameter("frame_id", "sonar"); - } - if (!this->has_parameter("frequency_mode")) { - rcl_interfaces::msg::ParameterDescriptor param_desc; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(1).set__to_value(2).set__step(1); - param_desc.name = "frequency_mode"; - param_desc.type = rclcpp::ParameterType::PARAMETER_INTEGER; - param_desc.description = "Sonar beam frequency mode.\n\t1: Low frequency (1.2MHz, wide aperture).\n\t2: High frequency (2.1Mhz, narrow aperture)."; - param_desc.integer_range = {range}; - this->declare_parameter("frequency_mode", 1, param_desc); - } - if (!this->has_parameter("ping_rate")) { - rcl_interfaces::msg::ParameterDescriptor param_desc; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(5).set__step(1); - param_desc.name = "ping_rate"; - param_desc.type = rclcpp::ParameterType::PARAMETER_INTEGER; - param_desc.description = "Frequency of ping fires.\n\t0: 10Hz max ping rate.\n\t1: 15Hz max ping rate.\n\t2: 40Hz max ping rate.\n\t3: 5Hz max ping rate.\n\t4: 2Hz max ping rate.\n\t5: Standby mode (no ping fire)."; - param_desc.integer_range = {range}; - this->declare_parameter("ping_rate", 0, param_desc); - } - if (!this->has_parameter("data_depth")) { - rcl_interfaces::msg::ParameterDescriptor param_desc; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(1).set__step(1); - param_desc.name = "data_depth"; - param_desc.type = rclcpp::ParameterType::PARAMETER_INTEGER; - param_desc.description = "Ping data encoding bit count.\n\t0: Ping data encoded on 8bits.\n\t1: Ping data encoded on 16bits."; - param_desc.integer_range = {range}; - this->declare_parameter("data_depth", 0, param_desc); - } - if (!this->has_parameter("nbeams")) { - rcl_interfaces::msg::ParameterDescriptor param_desc; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(1).set__step(1); - param_desc.name = "nbeams"; - param_desc.type = rclcpp::ParameterType::PARAMETER_INTEGER; - param_desc.description = "Number of ping beams.\n\t0: Oculus outputs 256 beams.\n\t1: Oculus outputs 512 beams."; - param_desc.integer_range = {range}; - this->declare_parameter("nbeams", 0, param_desc); - } - if (!this->has_parameter("send_gain")) { - rcl_interfaces::msg::ParameterDescriptor param_desc; - param_desc.name = "send_gain"; - param_desc.type = rclcpp::ParameterType::PARAMETER_BOOL; - param_desc.description = "Send range gain with data."; - this->declare_parameter("send_gain", false); - } - if (!this->has_parameter("gain_assist")) { - rcl_interfaces::msg::ParameterDescriptor param_desc; - param_desc.name = "gain_assist"; - param_desc.type = rclcpp::ParameterType::PARAMETER_BOOL; - param_desc.description = "Enable auto gain."; - this->declare_parameter("gain_assist", false, param_desc); - } - if (!this->has_parameter("range")) { - rcl_interfaces::msg::ParameterDescriptor param_desc; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.3).set__to_value(40.0).set__step(0.1); - param_desc.name = "nbeams"; - param_desc.type = rclcpp::ParameterType::PARAMETER_DOUBLE; - param_desc.description = "Sonar range (in meters), min=0.3, max=40.0."; - param_desc.floating_point_range = {range}; - this->declare_parameter("range", 3.0, param_desc); - } - if (!this->has_parameter("gamma_correction")) { - rcl_interfaces::msg::ParameterDescriptor param_desc; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(255).set__step(1); - param_desc.name = "gamma_correction"; - param_desc.type = rclcpp::ParameterType::PARAMETER_INTEGER; - param_desc.description = "Gamma correction, min=0, max=255."; - param_desc.integer_range = {range}; - this->declare_parameter("gamma_correction", 127, param_desc); - } - if (!this->has_parameter("gain_percent")) { - rcl_interfaces::msg::ParameterDescriptor param_desc; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.1).set__to_value(100.0).set__step(0.1); - param_desc.name = "gain_percent"; - param_desc.type = rclcpp::ParameterType::PARAMETER_DOUBLE; - param_desc.description = "Gain percentage (%), min=0.1, max=100.0."; - param_desc.floating_point_range = {range}; - this->declare_parameter("gain_percent", 50.0, param_desc); - } - if (!this->has_parameter("sound_speed")) { - rcl_interfaces::msg::ParameterDescriptor param_desc; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.0).set__to_value(1600.0).set__step(0.1); // min = 1400.0 but must include 0.0 for configuration - param_desc.name = "sound_speed"; - param_desc.type = rclcpp::ParameterType::PARAMETER_DOUBLE; - param_desc.description = "Sound speed (in m/s, set to 0 for it to be calculated using salinity), min=1400.0, max=1600.0."; - param_desc.floating_point_range = {range}; - this->declare_parameter("sound_speed", 0.0, param_desc); - } - if (!this->has_parameter("use_salinity")) { - rcl_interfaces::msg::ParameterDescriptor param_desc; - param_desc.name = "use_salinity"; - param_desc.type = rclcpp::ParameterType::PARAMETER_BOOL; - param_desc.description = "Use salinity to calculate sound_speed."; - this->declare_parameter("use_salinity", true, param_desc); +OculusSonarNode::OculusSonarNode() + : Node("oculus_sonar"), + is_running_(this->declare_parameter("run", params::RUN_MODE_DEFAULT_VALUE)), + sonar_viewer_(static_cast(this)), + frame_id_(this->declare_parameter("frame_id", "sonar")), + temperature_warn_limit_(this->declare_parameter("temperature_warn", params::TEMPERATURE_WARN_DEFAULT_VALUE)), + temperature_stop_limit_(this->declare_parameter("temperature_stop", params::TEMPERATURE_STOP_DEFAULT_VALUE)) { + this->status_publisher_ = this->create_publisher("status", 1); + this->ping_publisher_ = this->create_publisher("ping", 1); + this->temperature_publisher_ = this->create_publisher("temperature", 1); + this->pressure_publisher_ = this->create_publisher("pressure", 1); + + this->sonar_driver_ = std::make_shared(this->io_service_.io_service()); + this->io_service_.start(); + if (!this->sonar_driver_->wait_next_message()) { // Non-blocking function making connection with the sonar. + std::cerr << "Timeout reached while waiting for a connection to the Oculus sonar. " + << "Is it properly connected ?" << std::endl; + } + + while (!this->sonar_driver_->connected()) // Blocking while waiting the connected with the sonar. + { + // std::cerr << "Timeout reached while waiting for a connection to the Oculus sonar. " + // << "Is it properly connected ?" << std::endl; + const int sleepWhileConnecting = 1000; + std::this_thread::sleep_for(std::chrono::milliseconds(sleepWhileConnecting)); + } + + for (const params::BoolParam& param : params::BOOL) { + if (!this->has_parameter(param.name)) { + rcl_interfaces::msg::ParameterDescriptor param_desc; + param_desc.name = param.name; + param_desc.type = rclcpp::ParameterType::PARAMETER_BOOL; + param_desc.description = param.desc; + this->declare_parameter(param.name, param.default_val, param_desc); } - if (!this->has_parameter("salinity")) { - rcl_interfaces::msg::ParameterDescriptor param_desc; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.0).set__to_value(100.0).set__step(0.1); - param_desc.name = "salinity"; - param_desc.type = rclcpp::ParameterType::PARAMETER_DOUBLE; - param_desc.description = "Salinity (in parts per thousand (ppt,ppm,g/kg), used to calculate sound speed if needed), min=0.0, max=100"; - param_desc.floating_point_range = {range}; - this->declare_parameter("salinity", 0.0, param_desc); + } + for (const params::IntParam& param : params::INT) { + if (!this->has_parameter(param.name)) { + rcl_interfaces::msg::ParameterDescriptor param_desc; + param_desc.name = param.name; + param_desc.type = rclcpp::ParameterType::PARAMETER_INTEGER; + param_desc.description = param.desc; + rcl_interfaces::msg::IntegerRange range; + range.set__from_value(param.min).set__to_value(param.max).set__step(1); + param_desc.integer_range = {range}; + this->declare_parameter(param.name, param.default_val, param_desc); } - this->get_parameter("ping_topic", ping_topic_); - this->get_parameter("status_topic", status_topic_); - - this->param_cb_ = this->add_on_set_parameters_callback(std::bind(&OculusSonarNode::set_config_callback, this, std::placeholders::_1)); - - this->ping_publisher_ = this->create_publisher(ping_topic_, 100); - this->status_publisher_ = this->create_publisher(status_topic_, 100); - - this->sonar_driver_ = std::make_shared(this->io_service_.io_service()); - this->io_service_.start(); - if(!this->sonar_driver_->wait_next_message()) { - std::cerr << "Timeout reached while waiting for a connection to the Oculus sonar. " - << "Is it properly connected ?" << std::endl; + } + for (const params::DoubleParam& param : params::DOUBLE) { + if (!this->has_parameter(param.name)) { + rcl_interfaces::msg::ParameterDescriptor param_desc; + param_desc.name = param.name; + param_desc.type = rclcpp::ParameterType::PARAMETER_DOUBLE; + param_desc.description = param.desc; + rcl_interfaces::msg::IntegerRange range; + range.set__from_value(param.min).set__to_value(param.max).set__step(param.step); + param_desc.integer_range = {range}; + this->declare_parameter(param.name, param.default_val, param_desc); } - this->sonar_driver_->add_status_callback(std::bind(&OculusSonarNode::publish_status, this, std::placeholders::_1)); - this->sonar_driver_->add_ping_callback(std::bind(&OculusSonarNode::publish_ping, this, std::placeholders::_1, std::placeholders::_2)); - // callback on dummy messages to reactivate the pings as needed - this->sonar_driver_->add_dummy_callback(std::bind(&OculusSonarNode::handle_dummy, this)); + } + + // Get the current sonar config + updateLocalParameters(currentSonarParameters_, this->sonar_driver_->current_ping_config()); + for (const std::string& param_name : dynamic_parameters_names_) { + setConfigCallback(this->get_parameters(std::vector{param_name})); + } + this->param_cb_ = this->add_on_set_parameters_callback(std::bind(&OculusSonarNode::setConfigCallback, this, + std::placeholders::_1)); // TODO(hugoyvrn, to move before parameters initialisation ?) + + // this->??(&OculusSonarNode::enableRunMode) // TODO(hugoyvrn) + + this->sonar_driver_->add_status_callback(std::bind(&OculusSonarNode::publishStatus, this, std::placeholders::_1)); + this->sonar_driver_->add_ping_callback(std::bind(&OculusSonarNode::publishPing, this, std::placeholders::_1)); + // callback on dummy messages to reactivate the pings as needed + this->sonar_driver_->add_dummy_callback(std::bind(&OculusSonarNode::handleDummy, this)); } -OculusSonarNode::~OculusSonarNode() -{ - this->io_service_.stop(); +OculusSonarNode::~OculusSonarNode() { + this->io_service_.stop(); } -void OculusSonarNode::publish_status(const OculusStatusMsg& status) -{ - static oculus_interfaces::msg::OculusStatus msg; - - oculus::copy_to_ros(msg, status); +void OculusSonarNode::enableRunMode() { + this->sonar_driver_->resume(); // Quitting sonar standby mode + is_running_ = true; + this->set_parameter(rclcpp::Parameter("run", true)); +} - this->status_publisher_->publish(msg); +void OculusSonarNode::disableRunMode() { + this->sonar_driver_->standby(); // Going in sonar standby mode + RCLCPP_INFO(this->get_logger(), "Going to standby mode"); + is_running_ = false; + this->set_parameter(rclcpp::Parameter("run", false)); } -inline rclcpp::Time to_ros_stamp(const SonarDriver::TimePoint& stamp) -{ - size_t nano = std::chrono::duration_cast( - stamp.time_since_epoch()).count(); - size_t seconds = nano / 1000000000; - return rclcpp::Time(seconds, nano - 1000000000*seconds); +void OculusSonarNode::checkOverheating(const double& new_temperature) { + is_overheating_ = new_temperature >= temperature_stop_limit_; } -void OculusSonarNode::publish_ping(const OculusSimplePingResult& pingMetadata, - const std::vector& pingData) -{ - static oculus_interfaces::msg::OculusStampedPing msg; - - - if(this->count_subscribers(this->ping_topic_) == 0) { - cout << "Going to standby mode" << endl; - this->sonar_driver_->standby(); - //return; - } - - oculus::copy_to_ros(msg.ping, pingMetadata); - msg.ping.data.resize(pingData.size()); - for(int i = 0; i < msg.ping.data.size(); i++) - msg.ping.data[i] = pingData[i]; - - msg.header.stamp = to_ros_stamp(this->sonar_driver_->last_header_stamp()); - msg.header.frame_id = "oculus_sonar"; - this->ping_publisher_->publish(msg); +void OculusSonarNode::setMinimalFlags(uint8_t& flags) const { + flags |= flagByte::RANGE_AS_METERS // always in meters + | flagByte::SEND_GAINS // force send gain to true this + | flagByte::SIMPLE_PING; // use simple ping + + if (currentSonarParameters_.frequency_mode == params::FREQUENCY_MODE.max) { + // TODO(hugoyvrn, gain_assist not working, to fix) + // flags |= flagByte::GAIN_ASSIST; + flags &= ~flagByte::GAIN_ASSIST; + } + + // flags | 0x02 make wird change (depending of the configuration) + // flags |= 0x02; + flags &= ~0x02; + // flags | 0x20 must be ??? + // flags |= 0x20; + // flags &= ~0x20; + // flags | 0x80 must be false to avoid broken connection (Header reception error) and very long param answers. Restart of the + // sonar needed (even for IHM). tested with flags = 4d and flags = cd and flags = fd and flags = 19 + // flags |= 0x80; + flags &= ~0x80; } -void OculusSonarNode::handle_dummy() -{ - if(this->count_subscribers(this->ping_topic_) > 0) { - cout << "Exiting standby mode" << endl; - this->sonar_driver_->resume(); - } +void OculusSonarNode::checkMinimalFlags(const uint8_t& flags) const { + if (!(flags & flagByte::RANGE_AS_METERS)) { + RCLCPP_ERROR(get_logger(), "Range is attepreted as percent while ros driver assume range is interpreted as meters."); + } + if (!(flags & flagByte::SEND_GAINS)) { + RCLCPP_ERROR(get_logger(), "The sonar don't send gain while ros driver assume gains are sended. Data is incomplete."); + } + if (!(flags & flagByte::SIMPLE_PING)) { + RCLCPP_ERROR(get_logger(), "The sonar don't use simple ping message while ros driver assume simple ping are used."); + } } -rcl_interfaces::msg::SetParametersResult OculusSonarNode::set_config_callback(const std::vector ¶meters) -{ - SonarDriver::PingConfig currentConfig; - std::memset(¤tConfig, 0, sizeof(currentConfig)); - // flags - currentConfig.flags = 0x09; // always in meters, simple ping - - bool use_salinity; - for (const rclcpp::Parameter & param : parameters) { - // try { - if (param.get_name() == "frequency_mode") { - RCLCPP_INFO_STREAM(this->get_logger(), "Updating frequency_mode to " << param.as_int() << " (1: 1.2MHz, 2: 2.1MHz)."); - currentConfig.masterMode = param.as_int(); - } - else if (param.get_name() == "ping_rate") { - RCLCPP_INFO_STREAM(this->get_logger(), "Updating ping_rate to " << param.as_int() << " (0: 10Hz, 1: 15Hz, 2: 40Hz, 3: 5Hz, 4: 2Hz, 5: Standby mode)."); - switch(param.as_int()) - { - case 0: currentConfig.pingRate = pingRateNormal; break; // 10Hz - case 1: currentConfig.pingRate = pingRateHigh; break; // 15Hz - case 2: currentConfig.pingRate = pingRateHighest; break; // 40Hz - case 3: currentConfig.pingRate = pingRateLow; break; // 5Hz - case 4: currentConfig.pingRate = pingRateLowest; break; // 2Hz - case 5: currentConfig.pingRate = pingRateStandby; break; // standby mode - default:break; - } - } - else if (param.get_name() == "data_depth") { - RCLCPP_INFO_STREAM(this->get_logger(), "Updating data_depth to " << param.as_int() << " (0: 8 bits, 1: 16 bits)."); - switch(param.as_int()) - { - case 0: // 8 bits - break; - case 1: // 16 bits - currentConfig.flags |= 0x02; - break; - default:break; - } - } - else if (param.get_name() == "nbeams") { - RCLCPP_INFO_STREAM(this->get_logger(), "Updating nbeams to " << param.as_int() << " (0: 256 beams, 1: 512 beams)."); - switch(param.as_int()) - { - case 0: // 256 beams - break; - case 1: // 512 beams - currentConfig.flags |= 0x40; - break; - default:break; - } - } - else if (param.get_name() == "send_gain") { - RCLCPP_INFO_STREAM(this->get_logger(), "Updating send_gain to " << param.as_bool()); - if(param.as_bool()) - currentConfig.flags |= 0x04; - - } - else if (param.get_name() == "gain_assist") { - RCLCPP_INFO_STREAM(this->get_logger(), "Updating gain_assist to " << param.as_bool()); - if(param.as_bool()) - currentConfig.flags |= 0x10; - - } - else if (param.get_name() == "range") { - RCLCPP_INFO_STREAM(this->get_logger(), "Updating range to " << param.as_double() <<"m."); - currentConfig.range = param.as_double(); - - } - else if (param.get_name() == "gamma_correction") { - RCLCPP_INFO_STREAM(this->get_logger(), "Updating gamma_correction to " << param.as_int()); - currentConfig.gammaCorrection = param.as_int(); - - } - else if (param.get_name() == "gain_percent") { - RCLCPP_INFO_STREAM(this->get_logger(), "Updating gain_percent to " << param.as_double() << "%."); - currentConfig.gainPercent = param.as_double(); - - } - else if (param.get_name() == "use_salinity") { - RCLCPP_INFO_STREAM(this->get_logger(), "Updating use_salinity to " << param.as_bool()); - use_salinity = param.as_bool(); - if(use_salinity) - currentConfig.speedOfSound = 0.0; - - } - else if (param.get_name() == "sound_speed") { - RCLCPP_INFO_STREAM(this->get_logger(), "Updating sound_speed to " << param.as_double() << "m/s."); - if(!use_salinity) - { - if(param.as_double() >= 1400.0 && param.as_double() <= 1600.0) - currentConfig.speedOfSound = param.as_double(); - else - RCLCPP_INFO_STREAM(this->get_logger(), "Speed of sound must be between 1400.0 and 1600.0."); - } - - } else if (param.get_name() == "salinity") { - RCLCPP_INFO_STREAM(this->get_logger(), "Updating salinity to " << param.as_double() << " parts per thousand (ppt,ppm,g/kg)."); - currentConfig.salinity = param.as_double(); - } - // } catch (const std::runtime_error & e) { - // result.successful = false; - // RCLCPP_WARN(get_node()->get_logger(), "%s", e.what()); - // } - } - // send config to Oculus sonar and wait for feedback - auto feedback = this->sonar_driver_->request_ping_config(currentConfig); +void OculusSonarNode::publishStatus(const OculusStatusMsg& status) { + if (status.partNumber != OculusPartNumberType::partNumberM1200d) { + RCLCPP_ERROR_STREAM(get_logger(), + "The sonar version seems to be different than M1200d." + " This driver is not suppose to work with your sonar."); + } - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = ""; + // TODO(hugoyvrn, update ros param ?) - if(currentConfig.masterMode != feedback.masterMode) - { - result.successful = false; - result.reason.append("Could not update frequency_mode.\n"); - } - //currentConfig.pingRate != feedback.pingRate // is broken (?) sonar side - if((currentConfig.flags & 0x02) ? 1 : 0 != (feedback.flags & 0x02) ? 1 : 0) - { - result.successful = false; - result.reason.append("Could not update data_depth.\n"); - } - if((currentConfig.flags & 0x04) ? 1 : 0 != (feedback.flags & 0x04) ? 1 : 0) - { - result.successful = false; - result.reason.append("Could not update send_gain.\n"); - } - if((currentConfig.flags & 0x10) ? 1 : 0 != (feedback.flags & 0x10) ? 1 : 0) - { - result.successful = false; - result.reason.append("Could not update gain_assist.\n"); - } - if((currentConfig.flags & 0x40) ? 1 : 0 != (feedback.flags & 0x40) ? 1 : 0) - { - result.successful = false; - result.reason.append("Could not update nbeams.\n"); - } - if(currentConfig.range != feedback.range) - { - result.successful = false; - result.reason.append("Could not update range.\n"); - } - if(currentConfig.gammaCorrection != feedback.gammaCorrection) - { - result.successful = false; - result.reason.append("Could not update gamma_correction.\n"); + static oculus_interfaces::msg::OculusStatus msg; + oculus::toMsg(msg, status); + this->status_publisher_->publish(msg); + + if (!is_running_) { + checkOverheating(status.temperature6); + sensor_msgs::msg::Temperature temperature_ros_msg; + temperature_ros_msg.header.frame_id = frame_id_; + // temperature_ros_msg.header.stamp = this->get_clock()->now(); // TODO(hugoyvrn) + temperature_ros_msg.temperature = status.temperature6; // Measurement of the Temperature in Degrees Celsius + temperature_ros_msg.variance = 0; // 0 is interpreted as variance unknown + this->temperature_publisher_->publish(temperature_ros_msg); + + sensor_msgs::msg::FluidPressure pressure_ros_msg; + pressure_ros_msg.header.frame_id = frame_id_; + // pressure_ros_msg.header.stamp = this->get_clock()->now(); // TODO(hugoyvrn) + pressure_ros_msg.fluid_pressure = status.pressure; // Pressure reading in Pascals. + pressure_ros_msg.variance = 0; // 0 is interpreted as variance unknown + this->pressure_publisher_->publish(pressure_ros_msg); + } +} + +void OculusSonarNode::updateRosConfig() { + std::shared_lock l(param_mutex_); + + updateRosConfigForParam(currentRosParameters_.frequency_mode, currentSonarParameters_.frequency_mode, "frequency_mode"); + + updateRosConfigForParam(currentRosParameters_.range, currentSonarParameters_.range, "range"); + + updateRosConfigForParam(currentRosParameters_.gain_percent, currentSonarParameters_.gain_percent, "gain_percent"); + + updateRosConfigForParam(currentRosParameters_.sound_speed, currentSonarParameters_.sound_speed, "sound_speed"); + + updateRosConfigForParam(currentRosParameters_.ping_rate, currentSonarParameters_.ping_rate, "ping_rate"); + + updateRosConfigForParam(currentRosParameters_.gain_assist, currentSonarParameters_.gain_assist, "gain_assist"); + + updateRosConfigForParam( + currentRosParameters_.gamma_correction, currentSonarParameters_.gamma_correction, "gamma_correction"); + + updateRosConfigForParam(currentRosParameters_.use_salinity, currentSonarParameters_.use_salinity, "use_salinity"); + + updateRosConfigForParam(currentRosParameters_.salinity, currentSonarParameters_.salinity, "salinity"); +} + +int OculusSonarNode::get_subscription_count() const { + return this->ping_publisher_->get_subscription_count() + sonar_viewer_.image_publisher_->get_subscription_count(); +} + +void OculusSonarNode::publishPing(const oculus::PingMessage::ConstPtr& ping) { + // Check if the sonar must go in standby mode + checkOverheating(ping->temperature()); + if (!is_running_) { + disableRunMode(); + } else if (get_subscription_count() == 0) { + RCLCPP_INFO(this->get_logger(), "There is no subscriber nor to ping topic neither to image topic."); + disableRunMode(); + } else if (currentSonarParameters_.ping_rate == pingRateStandby) { + RCLCPP_INFO_STREAM(this->get_logger(), "ping_rate mode is seted to " << pingRateStandby << "."); + disableRunMode(); + } else if (is_overheating_) { + RCLCPP_FATAL_STREAM(this->get_logger(), "Temperature of sonar is to high (" + << ping->temperature() + << "°C). Make sur the sonar is underwatter. Security limit set at " + << temperature_stop_limit_ << "°C"); + disableRunMode(); + } else if (ping->temperature() >= temperature_warn_limit_) { + RCLCPP_WARN_STREAM(this->get_logger(), "Temperature of sonar is to high (" + << ping->temperature() + << "°C). Make sur the sonar is underwatter. Security limit set at " + << temperature_stop_limit_ << "°C"); + } + + // Update current config with ping information + currentSonarParameters_.frequency_mode = ping->master_mode(); + currentSonarParameters_.range = ping->range(); + currentSonarParameters_.gain_percent = ping->gain_percent(); + currentSonarParameters_.sound_speed = ping->speed_of_sound_used(); + updateRosConfig(); + + static oculus_interfaces::msg::Ping msg; + msg.header.frame_id = frame_id_; + oculus::toMsg(msg, ping); + this->ping_publisher_->publish(msg); + + sensor_msgs::msg::Temperature temperature_ros_msg; + temperature_ros_msg.header = msg.header; + temperature_ros_msg.temperature = msg.temperature; // Measurement of the Temperature in Degrees Celsius + temperature_ros_msg.variance = 0; // 0 is interpreted as variance unknown + this->temperature_publisher_->publish(temperature_ros_msg); + + sensor_msgs::msg::FluidPressure pressure_ros_msg; + pressure_ros_msg.header = msg.header; + pressure_ros_msg.fluid_pressure = msg.pressure; // Absolute pressure reading in Pascals. + pressure_ros_msg.variance = 0; // 0 is interpreted as variance unknown + this->pressure_publisher_->publish(pressure_ros_msg); + + // TODO(hugoyvrn, publish bearings) + + sonar_viewer_.publishFan(ping, frame_id_); +} + +void OculusSonarNode::handleDummy() { + if (is_running_ && get_subscription_count() > 0 && !is_overheating_ && currentSonarParameters_.ping_rate != pingRateStandby) { + RCLCPP_INFO(this->get_logger(), "Exiting standby mode"); + enableRunMode(); + } +} + +void OculusSonarNode::updateLocalParameters(SonarParameters& parameters, const std::vector& new_parameters) { + for (const rclcpp::Parameter& new_param : new_parameters) { + if (new_param.get_name() == "frequency_mode") { + parameters.frequency_mode = new_param.as_int(); + } else if (new_param.get_name() == "ping_rate") { + parameters.ping_rate = new_param.as_int(); + } else if (new_param.get_name() == "nbeams") { + parameters.nbeams = new_param.as_int(); + } else if (new_param.get_name() == "gain_assist") { + parameters.gain_assist = new_param.as_bool(); + } else if (new_param.get_name() == "range") { + parameters.range = new_param.as_double(); + } else if (new_param.get_name() == "gamma_correction") { + parameters.gamma_correction = new_param.as_int(); + } else if (new_param.get_name() == "gain_percent") { + parameters.gain_percent = new_param.as_double(); + } else if (new_param.get_name() == "sound_speed") { + parameters.sound_speed = new_param.as_double(); + } else if (new_param.get_name() == "use_salinity") { + parameters.use_salinity = new_param.as_bool(); + } else if (new_param.get_name() == "salinity") { + parameters.salinity = new_param.as_double(); + } else if (!(new_param.get_name() == "run")) { + RCLCPP_WARN_STREAM(get_logger(), "Wrong parameter to set : new_param = " << new_param << ". Not seted"); } - if(currentConfig.gainPercent != feedback.gainPercent) - { - result.successful = false; - result.reason.append("Could not update gain_percent.\n"); + } +} + +void OculusSonarNode::updateLocalParameters(SonarParameters& parameters, SonarDriver::PingConfig feedback) { + std::vector new_parameters; + // OculusMessageHeader head; // The standard message header + // uint16_t oculusId; // Fixed ID 0x4f53 + // uint16_t srcDeviceId; // The device id of the source + // uint16_t dstDeviceId; // The device id of the destination + // uint16_t msgId; // Message identifier + // uint16_t msgVersion; + // uint32_t payloadSize; // The size of the message payload (header not included) + // uint16_t spare2; + // uint8_t masterMode; // mode 0 is flexi mode, needs full fire message (not available for third party developers) + // // mode 1 - Low Frequency Mode (wide aperture, navigation) + // // mode 2 - High Frequency Mode (narrow aperture, target identification) + // uint8_t pingRate; // Sets the maximum ping rate. was PingRateType + // uint8_t networkSpeed; // Used to reduce the network comms speed (useful for high latency shared links) + // uint8_t gammaCorrection; // 0 and 0xff = gamma correction = 1.0 + // // Set to 127 for gamma correction = 0.5 + // uint8_t flags; // bit 0: 0 = interpret range as percent, 1 = interpret range as meters + // // bit 1: 0 = 8 bit data, 1 = 16 bit data // inverted ? + // // bit 2: 0 = won't send gain, 1 = send gain + // // bit 3: 0 = send full return message, 1 = send simple return message + // // bit 4: gain assist ? TODO + // // bit 5: ? TODO + // // bit 6: enable 512 beams + // // bit 7: ? TODO + // double range; // The range demand in percent or m depending on flags + // double gainPercent; // The gain demand + // double speedOfSound; // ms-1, if set to zero then internal calc will apply using salinity + // double salinity; // ppt, set to zero if we are in fresh water + + checkMinimalFlags(feedback.flags); + + new_parameters.push_back(rclcpp::Parameter("frequency_mode", feedback.masterMode)); // "frequency_mode" + new_parameters.push_back(rclcpp::Parameter("ping_rate", feedback.pingRate)); + new_parameters.push_back(rclcpp::Parameter("nbeams", static_cast(feedback.flags & flagByte::NBEAMS))); // nbeams + new_parameters.push_back( + rclcpp::Parameter("gain_assist", static_cast(feedback.flags & flagByte::GAIN_ASSIST))); // gain_assist + new_parameters.push_back(rclcpp::Parameter("range", feedback.range)); + new_parameters.push_back(rclcpp::Parameter("gamma_correction", feedback.gammaCorrection)); + new_parameters.push_back(rclcpp::Parameter("gain_percent", feedback.gainPercent)); + new_parameters.push_back(rclcpp::Parameter("sound_speed", feedback.speedOfSound)); + // // use_salinity // TODO(hugoyvrn) + // { + // rclcpp::Parameter param("use_salinity", ); + // new_parameters.push_back(param); + // } + + new_parameters.push_back(rclcpp::Parameter("salinity", feedback.salinity)); + updateLocalParameters(parameters, new_parameters); +} + +void OculusSonarNode::sendParamToSonar(rclcpp::Parameter param, rcl_interfaces::msg::SetParametersResult result) { + SonarDriver::PingConfig newConfig = currentConfig_; // To avoid to create a new SonarDriver::PingConfig from ros parameters + if (param.get_name() == "frequency_mode") { + RCLCPP_INFO_STREAM(this->get_logger(), "Updating frequency_mode to " << param.as_int() << " (1: 1.2MHz, 2: 2MHz)."); + newConfig.masterMode = param.as_int(); + } else if (param.get_name() == "ping_rate") { + RCLCPP_INFO_STREAM(this->get_logger(), "Updating ping_rate to " << param.as_int() << " (" + params::PING_RATE.desc + ")."); + newConfig.pingRate = param.as_int(); + } else if (param.get_name() == "nbeams") { + RCLCPP_INFO_STREAM(this->get_logger(), "Updating nbeams to " << param.as_int() << " (0: 256 beams, 1: 512 beams)."); + if (param.as_int() == 0) { + newConfig.flags &= ~flagByte::NBEAMS; // 256 beams + } else { + newConfig.flags |= flagByte::NBEAMS; // 512 beams } - if(currentConfig.speedOfSound != feedback.speedOfSound) - { - result.successful = false; - result.reason.append("Could not update sound_speed.\n"); + } else if (param.get_name() == "gain_assist") { + RCLCPP_INFO_STREAM(this->get_logger(), "Updating gain_assist to " << param.as_bool()); + if (param.as_bool()) { + newConfig.flags |= flagByte::GAIN_ASSIST; + } else { + newConfig.flags &= ~flagByte::GAIN_ASSIST; } - if(currentConfig.salinity != feedback.salinity) - { - result.successful = false; - result.reason.append("Could not update salinity.\n"); + } else if (param.get_name() == "range") { + RCLCPP_INFO_STREAM(this->get_logger(), "Updating range to " << param.as_double() << "m."); + newConfig.range = param.as_double(); + } else if (param.get_name() == "gamma_correction") { + RCLCPP_INFO_STREAM(this->get_logger(), "Updating gamma_correction to " << param.as_int()); + newConfig.gammaCorrection = param.as_int(); + } else if (param.get_name() == "gain_percent") { + RCLCPP_INFO_STREAM(this->get_logger(), "Updating gain_percent to " << param.as_double() << "%."); + newConfig.gainPercent = param.as_double(); + } else if (param.get_name() == "use_salinity") { + RCLCPP_INFO_STREAM(this->get_logger(), "Updating use_salinity to " << param.as_bool()); + if (param.as_bool()) + newConfig.speedOfSound = 0.0; + } else if (param.get_name() == "sound_speed") { + RCLCPP_INFO_STREAM(this->get_logger(), "Updating sound_speed to " << param.as_double() << " m/s."); + if (!currentRosParameters_.use_salinity) { + if (param.as_double() >= 1400.0 && + param.as_double() <= 1600.0) // TODO(hugoyvrn, why is there a range verification here and not for other parameters?) + newConfig.speedOfSound = param.as_double(); + else + RCLCPP_INFO_STREAM(this->get_logger(), "Speed of sound must be between 1400.0 and 1600.0."); } - //(currentConfig.flags & 0x08) ? 1 : 0 != (feedback.flags & 0x08) ? 1 : 0 + } else if (param.get_name() == "salinity") { + RCLCPP_INFO_STREAM(this->get_logger(), "Updating salinity to " << param.as_double() << " parts per thousand (ppt,ppm,g/kg)."); + newConfig.salinity = param.as_double(); + } + + setMinimalFlags(newConfig.flags); + + // send config to Oculus sonar and wait for feedback + SonarDriver::PingConfig feedback = this->sonar_driver_->request_ping_config(newConfig); + currentConfig_ = feedback; + + updateLocalParameters(currentSonarParameters_, feedback); + + checkMinimalFlags(feedback.flags); + + handleFeedbackForParam(result, param, newConfig.masterMode, feedback.masterMode, "frequency_mode"); + // newConfig.pingRate != feedback.pingRate // is broken (?) sonar side TODO(???) + handleFeedbackForParam(result, param, (newConfig.flags & flagByte::GAIN_ASSIST) ? 1 : 0, + (feedback.flags & flagByte::GAIN_ASSIST) ? 1 : 0, "gain_assist"); + handleFeedbackForParam( + result, param, (newConfig.flags & flagByte::NBEAMS) ? 1 : 0, (feedback.flags & flagByte::NBEAMS) ? 1 : 0, "nbeams"); + handleFeedbackForParam(result, param, newConfig.range, feedback.range, "range"); + handleFeedbackForParam(result, param, newConfig.gammaCorrection, feedback.gammaCorrection, "gamma_correction"); + handleFeedbackForParam(result, param, newConfig.gainPercent, feedback.gainPercent, "gain_percent"); + handleFeedbackForParam(result, param, newConfig.speedOfSound, feedback.speedOfSound, "sound_speed"); + handleFeedbackForParam(result, param, newConfig.salinity, feedback.salinity, "salinity"); +} + +rcl_interfaces::msg::SetParametersResult OculusSonarNode::setConfigCallback(const std::vector& parameters) { + std::shared_lock l(param_mutex_); + if (parameters.size() != 1) { + RCLCPP_WARN(get_logger(), "You should set parameters one by one."); + RCLCPP_INFO_STREAM(get_logger(), "parameters = " << parameters); + rcl_interfaces::msg::SetParametersResult result; + result.successful = false; + result.reason = "Parameters should be set one by one"; return result; + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = ""; + + for (const rclcpp::Parameter& param : parameters) { + if (param.get_name() == "run") { + if (!is_running_ || param.as_bool()) { + if (get_subscription_count() == 0 || is_overheating_ || currentSonarParameters_.ping_rate == pingRateStandby) { + result.successful = false; + result.reason = "The condition to go in run mode are not meeted."; + if (get_subscription_count() == 0) { + result.reason += " There is no subscriber nor to ping topic neither to image topic."; + } + if (is_overheating_) { + result.reason += + " Temperature of sonar is to high." + " Make sur the sonar is underwatter. Security limit set at " + + std::to_string(temperature_stop_limit_) + "°C"; + } + if (currentSonarParameters_.ping_rate == pingRateStandby) { + result.reason += " ping_rate mode is seted to " + std::to_string(pingRateStandby) + "."; + } + return result; + } + } + is_running_ = param.as_bool(); + + } else if (std::find(dynamic_parameters_names_.begin(), dynamic_parameters_names_.end(), param.get_name()) != + dynamic_parameters_names_.end()) { + sendParamToSonar(param, result); + } + } + + if (result.successful) { // If the parameters will be updated to ros + updateLocalParameters(currentRosParameters_, parameters); + } + + return result; } -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); // force to monothread + rclcpp::shutdown(); + return 0; } diff --git a/oculus_ros2/src/oculus_sonar_node.hpp b/oculus_ros2/src/oculus_sonar_node.hpp deleted file mode 100644 index 854edf9..0000000 --- a/oculus_ros2/src/oculus_sonar_node.hpp +++ /dev/null @@ -1,42 +0,0 @@ -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" - -#include "conversions.h" - -#include -#include - -#include "oculus_interfaces/msg/oculus_status.hpp" -#include "oculus_interfaces/msg/oculus_stamped_ping.hpp" - -#include "rcl_interfaces/msg/parameter_descriptor.hpp" - -class OculusSonarNode : public rclcpp::Node -{ - public: - OculusSonarNode(); - ~OculusSonarNode(); - - - private: - std::shared_ptr sonar_driver_; - oculus::AsyncService io_service_; - - std::string ping_topic_ = "ping"; - std::string status_topic_ = "status"; - rclcpp::Publisher::SharedPtr status_publisher_{nullptr}; - rclcpp::Publisher::SharedPtr ping_publisher_{nullptr}; - - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_{nullptr}; - - rcl_interfaces::msg::SetParametersResult set_config_callback(const std::vector & parameters); - - void publish_status(const OculusStatusMsg& status); - void publish_ping(const OculusSimplePingResult& pingMetadata, - const std::vector& pingData); - void handle_dummy(); -}; diff --git a/oculus_ros2/src/oculus_viewer_node.cpp b/oculus_ros2/src/oculus_viewer_node.cpp new file mode 100644 index 0000000..b84ea82 --- /dev/null +++ b/oculus_ros2/src/oculus_viewer_node.cpp @@ -0,0 +1,56 @@ +/** + * BSD 3-Clause License + * + * Copyright (c) 2022, ENSTA-Bretagne + * 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 the copyright holder 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 HOLDER 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. + */ + +#define BOOST_BIND_GLOBAL_PLACEHOLDERS + +#include + +using SonarDriver = oculus::SonarDriver; + +OculusViewerNode::OculusViewerNode() : Node("oculus_viewer"), sonar_viewer_(static_cast(this)) { + ping_subscription_ = this->create_subscription( + "ping", 10, std::bind(&OculusViewerNode::pingCallback, this, std::placeholders::_1)); +} + +OculusViewerNode::~OculusViewerNode() {} + +void OculusViewerNode::pingCallback(const oculus_interfaces::msg::Ping& ping_msg) const { + // RCLCPP_INFO(get_logger(), "I am in callback"); + sonar_viewer_.publishFan(ping_msg); +} + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/oculus_ros2/src/sonar_viewer.cpp b/oculus_ros2/src/sonar_viewer.cpp new file mode 100644 index 0000000..755294d --- /dev/null +++ b/oculus_ros2/src/sonar_viewer.cpp @@ -0,0 +1,106 @@ +/** + * BSD 3-Clause License + * + * Copyright (c) 2022, ENSTA-Bretagne + * 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 the copyright holder 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 HOLDER 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. + */ + +#include + +SonarViewer::SonarViewer(rclcpp::Node* node) : node_(node) { + image_publisher_ = node->create_publisher("image", 10); +} + +SonarViewer::~SonarViewer() {} + +void SonarViewer::publishFan(const oculus_interfaces::msg::Ping& ros_ping_msg) const { + // const int offset = ping->ping_data_offset(); // TODO(hugoyvrn) + const int offset = 229; // quick fix TODO(hugoyvrn, why 229?) + + publishFan( + ros_ping_msg.n_beams, ros_ping_msg.n_ranges, offset, ros_ping_msg.ping_data, ros_ping_msg.master_mode, ros_ping_msg.header); +} + +void SonarViewer::publishFan(const oculus::PingMessage::ConstPtr& ping, const std::string& frame_id) const { + std_msgs::msg::Header header; + header.stamp = oculus::toMsg(ping->timestamp()); + header.frame_id = frame_id; + + publishFan(ping->bearing_count(), ping->range_count(), ping->ping_data_offset(), ping->data(), ping->master_mode(), header); +} + +void SonarViewer::publishFan(const int& width, + const int& height, + const int& offset, + const std::vector& ping_data, + const int& master_mode, + const std_msgs::msg::Header& header) const { + const int step = width + SIZE_OF_GAIN_; + const float theta_shift = 270; + const int mat_encoding = CV_8U; + const char* ros_image_encoding = sensor_msgs::image_encodings::MONO8; + + cv::Mat rawDataMat(height, step, mat_encoding); + + std::memcpy(rawDataMat.data, ping_data.data() + offset, height * step); + + const double bearing = (master_mode == 1) ? LOW_FREQUENCY_BEARING_APERTURE_ : HIGHT_FREQUENCY_BEARING_APERTURE_; + const int image_width = 2 * std::sin(bearing * M_PI / 180) * height; + cv::Mat mono_img = + cv::Mat::ones(cv::Size(image_width, height), CV_MAKETYPE(mat_encoding, 1)) * std::numeric_limits::max(); + const cv::Point origin(image_width / 2, height); + + cv::parallel_for_(cv::Range(0, height), [&](const cv::Range& range) { // TODO(??, optimize for cuda) + for (int r = range.start; r < range.end; r++) { + std::vector pts; + cv::ellipse2Poly(origin, cv::Size(r, r), theta_shift, -bearing, bearing, 1, pts); + + std::vector arc_points; + arc_points.push_back(pts[0]); + + for (size_t k = 0; k < (pts.size() - 1); k++) { + cv::LineIterator it(mono_img, pts[k], pts[k + 1], SIZE_OF_GAIN_); + for (int i = 1; i < it.count; i++, ++it) { + arc_points.push_back(it.pos()); + } + } + + cv::Mat data_rows_resized; + cv::resize(rawDataMat.row(r), data_rows_resized, cv::Size(arc_points.size(), arc_points.size())); + + for (size_t k = 0; k < arc_points.size(); k++) { + mono_img.at(arc_points[k]) = data_rows_resized.at(1, k); + } + } + }); + + // Publish sonar conic image + sensor_msgs::msg::Image msg; + cv_bridge::CvImage(header, ros_image_encoding, mono_img).toImageMsg(msg); + image_publisher_->publish(msg); +} diff --git a/oculus_ros2/tests/python/config01.py b/oculus_ros2/tests/python/config01.py old mode 100644 new mode 100755 index ff00320..13eb529 --- a/oculus_ros2/tests/python/config01.py +++ b/oculus_ros2/tests/python/config01.py @@ -1,19 +1,28 @@ #! /usr/bin/python +# Copyright 2023 Forssea Robotics +# All rights reserved. +# +# Unauthorized copying of this code base via any medium is strictly prohibited. +# Proprietary and confidential. + import rospy import dynamic_reconfigure.client config = 0 + + def callback(cfg): print("Got config") global config config = cfg + rospy.init_node("sonar_ctrl1") -client = dynamic_reconfigure.client.Client("oculus_sonar", timeout=20, config_callback=callback) +client = dynamic_reconfigure.client.Client( + "oculus_sonar", timeout=20, config_callback=callback +) desc = client.get_parameter_descriptions() # client.update_configuration({'frequency_mode':2}) # print(config) - - diff --git a/oculus_ros2/tests/python/print_config.py b/oculus_ros2/tests/python/print_config.py old mode 100644 new mode 100755 index dc5c28b..ad5369f --- a/oculus_ros2/tests/python/print_config.py +++ b/oculus_ros2/tests/python/print_config.py @@ -1,13 +1,43 @@ #! /usr/bin/python +# BSD 3-Clause License +# +# Copyright (c) 2022, ENSTA-Bretagne +# 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 the copyright holder 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 HOLDER 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. + from oculus_sonar.cfg import OculusSonarConfig as config + def print_parameter(param): - print("Parameter : " + param['name'] + " :") + print("Parameter : " + param["name"] + " :") for key in param: print(" - " + key + " : " + str(param[key])) -for param in config.config_description['parameters']: - print_parameter(param) - +for param in config.config_description["parameters"]: + print_parameter(param) diff --git a/oculus_ros2/tests/python/subscriber01.py b/oculus_ros2/tests/python/subscriber01.py old mode 100644 new mode 100755 index 85a3bb3..6da2c84 --- a/oculus_ros2/tests/python/subscriber01.py +++ b/oculus_ros2/tests/python/subscriber01.py @@ -1,5 +1,11 @@ #! /usr/bin/python +# Copyright 2023 Forssea Robotics +# All rights reserved. +# +# Unauthorized copying of this code base via any medium is strictly prohibited. +# Proprietary and confidential. + # import roslibpy # client = roslibpy.Ros(host='localhost', port=11311) # client.run() @@ -7,12 +13,13 @@ import rospy import oculus_sonar.msg as oculus_msg + def callback(data): data.data = [] print(data, flush=True) -rospy.init_node('sonar_listener', anonymous=True) -rospy.Subscriber('/ping', oculus_msg.OculusPing, callback) -# rospy.spin() +rospy.init_node("sonar_listener", anonymous=True) +rospy.Subscriber("/ping", oculus_msg.OculusPing, callback) +# rospy.spin() diff --git a/oculus_sonar/CMakeLists.txt b/oculus_sonar/CMakeLists.txt new file mode 100644 index 0000000..04dda9f --- /dev/null +++ b/oculus_sonar/CMakeLists.txt @@ -0,0 +1,5 @@ +cmake_minimum_required(VERSION 3.5) +project(oculus_sonar) + +find_package(ament_cmake REQUIRED) +ament_package() diff --git a/oculus_sonar/package.xml b/oculus_sonar/package.xml new file mode 100644 index 0000000..59a3e28 --- /dev/null +++ b/oculus_sonar/package.xml @@ -0,0 +1,20 @@ + + + oculus_sonar + 0.0.0 + ROS 2 package for BluePrint subsea Oculus M1200d sonar + Auguste Bourgois + + proprietary + + ament_cmake + + oculus_driver + oculus_interfaces + oculus_ros2 + oculus_image_converter + + + ament_cmake + +