diff --git a/.clang-format b/.clang-format index 817fd6d..4d2e393 100644 --- a/.clang-format +++ b/.clang-format @@ -50,16 +50,14 @@ SpaceAfterCStyleCast: false BreakBeforeBraces: Custom # Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} -... +BraceWrapping: + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' \ No newline at end of file diff --git a/.gitignore b/.gitignore index b2f99f6..8868941 100644 --- a/.gitignore +++ b/.gitignore @@ -12,31 +12,6 @@ # Built Visual Studio Code Extensions *.vsix - -# ROS -devel/ -logs/ -build/ -bin/ -lib/ -msg_gen/ -srv_gen/ -msg/*Action.msg -msg/*ActionFeedback.msg -msg/*ActionGoal.msg -msg/*ActionResult.msg -msg/*Feedback.msg -msg/*Goal.msg -msg/*Result.msg -msg/_*.py -build_isolated/ -devel_isolated/ - -# Generated by dynamic reconfigure -*.cfgc -/cfg/cpp/ -/cfg/*.py - # Ignore generated docs *.dox *.wikidoc @@ -63,5 +38,5 @@ qtcreator-* # Emacs .#* -# Catkin custom files -CATKIN_IGNORE \ No newline at end of file +# Builds +build*/ diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml new file mode 100644 index 0000000..2b90ff2 --- /dev/null +++ b/.gitlab-ci.yml @@ -0,0 +1,72 @@ +# General concept is strongly inspired by the industrial_ci project https://github.com/ros-industrial/industrial_ci +--- +variables: + GIT_STRATEGY: clone + +stages: +- build + +.build_matrix: + parallel: + matrix: + - RUN_NAME: "22.04+1.4.3" # No effect on CI, just for making the gitlab pipeline view easier to read + CI_IMAGE: ros:humble-ros-core-jammy + IFM3D_PACKAGE_PATH: https://github.com/ifm/ifm3d/releases/download/v1.4.3/ifm3d-ubuntu-22.04-amd64-debs_1.4.3.tar + ROS_DISTRO: humble + - RUN_NAME: "22.04+1.5.0" + CI_IMAGE: ros:humble-ros-core-jammy + IFM3D_PACKAGE_PATH: https://github.com/ifm/ifm3d/releases/download/v1.5.0/ifm3d-ubuntu-22.04-amd64-debs_1.5.0.tar + ROS_DISTRO: humble + - RUN_NAME: "22.04+1.5.1" + CI_IMAGE: ros:humble-ros-core-jammy + IFM3D_PACKAGE_PATH: https://github.com/ifm/ifm3d/releases/download/v1.5.1/ifm3d-ubuntu-22.04-amd64-debs_1.5.1.tar + ROS_DISTRO: humble + - RUN_NAME: "22.04+1.5.2" + CI_IMAGE: ros:humble-ros-core-jammy + IFM3D_PACKAGE_PATH: https://github.com/ifm/ifm3d/releases/download/v1.5.2/ifm3d-ubuntu-22.04-amd64-debs_1.5.2.tar + ROS_DISTRO: humble + - RUN_NAME: "22.04+1.5.3" + CI_IMAGE: ros:humble-ros-core-jammy + IFM3D_PACKAGE_PATH: https://github.com/ifm/ifm3d/releases/download/v1.5.3/ifm3d-ubuntu-22.04-amd64-debs_1.5.3.tar + ROS_DISTRO: humble + + - RUN_NAME: "24.04+1.5.3" + CI_IMAGE: ros:jazzy-ros-core-noble + IFM3D_PACKAGE_PATH: https://github.com/ifm/ifm3d/releases/download/v1.5.3/ifm3d-ubuntu-22.04-amd64-debs_1.5.3.tar + ROS_DISTRO: jazzy + + +build: # very short job name to keep the pipeline preview readable on Gitlab + stage: build + image: $CI_IMAGE + parallel: !reference [.build_matrix,parallel] + + before_script: + # Download the released version ofifm3d from github + - apt-get update + - apt-get install -y curl + - curl --location $IFM3D_PACKAGE_PATH | tar x + # Install dependencies + - apt-get install -y libboost-all-dev git libcurl4-openssl-dev libgtest-dev libgoogle-glog-dev + libxmlrpc-c++8-dev libopencv-dev libpcl-dev libproj-dev python3-dev python3-pip build-essential + coreutils findutils cmake locales ninja-build + # Install ifm3d packages + - dpkg -i ./ifm3d_*.deb + # Cleanup + - rm ifm3d_*.deb + + script: + # Install some dependencies, update rosdep + - apt-get update + - apt-get install -y ros-dev-tools + - rosdep init || true # init needed for 22.04 image but throws error on 24.04; error can be ignored + - rosdep update > /dev/null + # Create Workspace and copy sources + - mkdir -p /root/target_ws/src + - cp -r . /root/target_ws/src + - cd /root/target_ws/ + # Install dependencies, build workspace and run tests + - source /opt/ros/$ROS_DISTRO/setup.bash + - rosdep install --from-paths /root/target_ws/src --ignore-src -y + - colcon build --event-handlers desktop_notification- status- terminal_title- --cmake-args -DENABLE_COVERAGE_TESTING=ON -DCMAKE_BUILD_TYPE=Debug -DCMAKE_EXPORT_COMPILE_COMMANDS=ON + - colcon test --event-handlers desktop_notification- status- terminal_title- console_cohesion+ --executor sequential --ctest-args -j1 \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..4a7d3f3 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,85 @@ +{ + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "any": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "coroutine": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "regex": "cpp", + "source_location": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "ranges": "cpp", + "semaphore": "cpp", + "shared_mutex": "cpp", + "span": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "valarray": "cpp", + "variant": "cpp", + "*.ipp": "cpp" + } +} \ No newline at end of file diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 9f95838..ca176ea 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,33 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package ifm3d-ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^ +1.2 +=== + +1.2.0 +----- +* Create an ODS node to publish ODS data: + * The launch file `ods.launch.py` can be used, + * Add two topics, `"~/ods_info"` and `"~/ods_occupancy_map_ros"`, + * An example launch configuration for ODS is provided `ods_default_parameters.yaml` and can be used with the launch file, + * It is expected that ODS is configured before the node is launched. Alternatively, one can use the new `config_file` parameter. +* Remove the `diag_mode` parameter: diagnostic is always polled periodically and published to `"/diagnostic"` +* Add the `GetDiag` service for polling filtered diagnostic data. +* Camera info topic: + * Add a `"~/camera_info"` topic for RGB cameras. + * The `"~/camera_info"` topic for the TOF cameras is published when the `TOF_INFO` buffer is requested, instead of the `INTRINSIC` buffer. +* Add a `config_file` parameter. It should be formatted in JSON and will be used to configure the device when the `CONFIGURE` state is triggered. +* Transforms: + * The `cloud_link` was renamed to `ifm_base_link`, and is used as the reference ifm calibrated coordinate system for all ifm data (RGB, 3D and ODS). + * The transforms between the `cloud_link` (now `ifm_base_link`), and the `mounting_link` and `optical_link` are fixed. + * The `mounting_link` to `optical_link` transform is read when the first `TOF_INFO` or `RGB_INFO` buffer is received, and remains constant. + * The `ifm_base_link` to `mounting_link` transform is published once when the first `TOF_INFO` or `RGB_INFO` buffer is received, and is only re-published subsequently if changed. + * The camera node parameters related to tf publication got reworked, the new parameters are: + * `tf.base_frame_name`: Name for ifm reference frame +| * `tf.mounting_frame_name`: Name for the mounting point frame +| * `tf.optical_frame_name`: Name for the optical frame +| * `tf.publish_base_to_mounting`: Whether the transform from the ifm base link to the camera mounting point should be published +| * `tf.publish_mounting_to_optical`: Whether the transform from the cameras mounting point to the optical center should be published 1.1 === diff --git a/CMakeLists.txt b/CMakeLists.txt index d38cee8..e32dad4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,8 @@ set(IFM3D_ROS2_DEPS diagnostic_msgs geometry_msgs lifecycle_msgs + nav_msgs + nav2_msgs rclcpp rclcpp_components rclcpp_lifecycle @@ -40,8 +42,10 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/InverseIntrinsics.msg" "msg/RGBInfo.msg" "msg/TOFInfo.msg" + "msg/Zones.msg" "srv/Dump.srv" "srv/Config.srv" + "srv/GetDiag.srv" "srv/Softoff.srv" "srv/Softon.srv" DEPENDENCIES builtin_interfaces std_msgs @@ -53,11 +57,38 @@ rosidl_generate_interfaces(${PROJECT_NAME} include_directories(include) -# +# Target for messages included in this package +rosidl_get_typesupport_target(msgs_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + + +add_library(ifm3d_ros2_buffer_conversions SHARED + src/lib/buffer_conversions.cpp) +target_link_libraries(ifm3d_ros2_buffer_conversions + "${msgs_typesupport_target}" + ifm3d::framegrabber) +ament_target_dependencies(ifm3d_ros2_buffer_conversions ${IFM3D_ROS2_DEPS}) + +add_library(ifm3d_ros2_buffer_id_utils SHARED + src/lib/buffer_id_utils.cpp) +target_link_libraries(ifm3d_ros2_buffer_id_utils + "${msgs_typesupport_target}" + ifm3d::framegrabber) +ament_target_dependencies(ifm3d_ros2_buffer_id_utils ${IFM3D_ROS2_DEPS}) + # ifm3d camera "component" (.so) state machine ("lifecycle node") # -add_library(ifm3d_ros2_camera_node SHARED src/lib/camera_node.cpp) +add_library(ifm3d_ros2_camera_node SHARED + src/lib/camera_node.cpp + src/lib/diag_module.cpp + src/lib/function_module.cpp + src/lib/rgb_module.cpp + src/lib/camera_tf_publisher.cpp + src/lib/tof_module.cpp + src/lib/services.cpp) target_link_libraries(ifm3d_ros2_camera_node + "${msgs_typesupport_target}" + ifm3d_ros2_buffer_conversions + ifm3d_ros2_buffer_id_utils ifm3d::device ifm3d::framegrabber ) @@ -65,8 +96,26 @@ ament_target_dependencies(ifm3d_ros2_camera_node ${IFM3D_ROS2_DEPS}) rclcpp_components_register_nodes( ifm3d_ros2_camera_node "ifm3d_ros2::CameraNode" ) -rosidl_target_interfaces(ifm3d_ros2_camera_node - ${PROJECT_NAME} "rosidl_typesupport_cpp" + +# +# ifm3d ods node +# +add_library(ifm3d_ros2_ods_node SHARED + src/lib/ods_node.cpp + src/lib/function_module.cpp + src/lib/diag_module.cpp + src/lib/ods_module.cpp + src/lib/services.cpp) +target_link_libraries(ifm3d_ros2_ods_node + "${msgs_typesupport_target}" + ifm3d_ros2_buffer_conversions + ifm3d_ros2_buffer_id_utils + ifm3d::device + ifm3d::framegrabber + ) +ament_target_dependencies(ifm3d_ros2_ods_node ${IFM3D_ROS2_DEPS}) +rclcpp_components_register_nodes( + ifm3d_ros2_ods_node "ifm3d_ros2::OdsNode" ) # @@ -75,36 +124,37 @@ rosidl_target_interfaces(ifm3d_ros2_camera_node # launch_ros). # ament_auto_add_executable(camera_standalone src/bin/camera_standalone.cpp) -target_link_libraries(camera_standalone ifm3d_ros2_camera_node) +target_link_libraries(camera_standalone + "${msgs_typesupport_target}" + ifm3d_ros2_camera_node) ament_target_dependencies(camera_standalone ${IFM3D_ROS2_DEPS}) -rosidl_target_interfaces(camera_standalone - ${PROJECT_NAME} "rosidl_typesupport_cpp" - ) - -find_package(Boost 1.65.0 REQUIRED COMPONENTS system) target_link_libraries(ifm3d_ros2_camera_node ${Boost_LIBRARIES}) + +ament_auto_add_executable(ods_standalone src/bin/ods_standalone.cpp) +target_link_libraries(ods_standalone + "${msgs_typesupport_target}" + ifm3d_ros2_ods_node) +ament_target_dependencies(ods_standalone ${IFM3D_ROS2_DEPS}) +target_link_libraries(ifm3d_ros2_ods_node ${Boost_LIBRARIES}) # TODO needed? + + ############## ## Install ## ############## install( - TARGETS camera_standalone + TARGETS camera_standalone ods_standalone DESTINATION lib/${PROJECT_NAME} ) install( - TARGETS ifm3d_ros2_camera_node + TARGETS ifm3d_ros2_camera_node ifm3d_ros2_ods_node ifm3d_ros2_buffer_conversions ifm3d_ros2_buffer_id_utils ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -install( - PROGRAMS scripts/dump scripts/config - DESTINATION lib/${PROJECT_NAME}/ - ) - install( DIRECTORY launch DESTINATION share/${PROJECT_NAME}/ @@ -115,15 +165,6 @@ install( DESTINATION share/${PROJECT_NAME}/ ) -install( - DIRECTORY examples - DESTINATION share/${PROJECT_NAME}/ - PATTERN "examples/*.py" - PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ - GROUP_EXECUTE GROUP_READ - WORLD_EXECUTE WORLD_READ - ) - install( DIRECTORY config DESTINATION share/${PROJECT_NAME}/ diff --git a/Dockerfile b/Dockerfile index 00443c2..cdc85ee 100644 --- a/Dockerfile +++ b/Dockerfile @@ -26,17 +26,12 @@ RUN apt-get update && apt-get install -y \ coreutils \ git \ jq \ - libcurl4-openssl-dev \ - libboost-all-dev \ - libgoogle-glog-dev \ - libgoogle-glog0v5 \ - libproj-dev \ libssl-dev \ - libxmlrpc-c++8-dev \ + libxmlrpc-c++8v5 \ wget \ && rm -rf /var/lib/apt/lists/* -# Update libcurl4 library for the dependancy of ifm3dAPI 1.4.3 & requires libcurl4 version 16 -RUN apt-get update && apt-get upgrade libcurl4 +# Update libcurl4 library for the dependancy of ifm3dAPI 1.5.3 & requires libcurl4 version 16 +RUN apt-get update && apt-get upgrade -y libcurl4 # Install ifm3d using the deb files RUN mkdir /home/ifm/ifm3d ADD https://github.com/ifm/ifm3d/releases/download/v${IFM3D_VERSION}/ifm3d-ubuntu-${UBUNTU_VERSION}-${ARCH}-debs_${IFM3D_VERSION}.tar /home/ifm/ifm3d @@ -51,10 +46,10 @@ RUN cd /home/ifm/colcon_ws && \ rosdep install --from-path src -y --ignore-src -t build # OPTION 2: clone and build from git repo, i.e. download specific repo branch during build process -# # Clone and build ifm3d-ros2 repo -# RUN mkdir -p /home/ifm/colcon_ws/src && \ -# cd /home/ifm/colcon_ws/src && \ -# git clone ${IFM3D_ROS2_REPO} -b ${IFM3D_ROS2_BRANCH} --single-branch +# Clone and build ifm3d-ros2 repo +#RUN mkdir -p /home/ifm/colcon_ws/src && \ +# cd /home/ifm/colcon_ws/src && \ +# git clone ${IFM3D_ROS2_REPO} -b ${IFM3D_ROS2_BRANCH} --single-branch SHELL ["/bin/bash", "-c"] RUN cd /home/ifm/colcon_ws && \ @@ -75,16 +70,14 @@ WORKDIR /home/ifm ARG DEBIAN_FRONTEND=noninteractive RUN apt-get update \ && apt-get install -y --no-install-recommends \ - libboost-all-dev \ - libgoogle-glog0v5 \ libssl-dev \ libxmlrpc-c++8v5 \ locales \ python3-rosdep \ && rm -rf /var/lib/apt/lists/* -# Update libcurl4 library for the dependancy of ifm3dAPI 1.4.3 & requires libcurl4 version 16 -RUN apt-get update && apt-get upgrade libcurl4 +# Update libcurl4 library for the dependancy of ifm3dAPI 1.5.3 & requires libcurl4 version 16 +RUN apt-get update && apt-get upgrade -y libcurl4 # Install ifm3d RUN cd /home/ifm/ifm3d &&\ diff --git a/README.md b/README.md index b5b65ee..bac2006 100644 --- a/README.md +++ b/README.md @@ -1,49 +1,49 @@ # ifm3d-ros2 overview *This documentation is formatted to be read on [www.ros2.ifm3d.com](https://ros2.ifm3d.com/latest/).* -:::{warning} -The ifm3d-ros2 package has had major changes recently. Please be aware that this might cause problems on your system for building pipelines based on our old build instructions. -::: - - :::{note} -This release is intended to be used with the O3R camera platform ONLY. For other ifm cameras (e.g. O3D3xx and O3X2xx) please see the tagged releases 0.3.0 and 0.7.0 respectively. +This release is intended to be used with the O3R camera platform ONLY. For other ifm cameras (e.g. O3D3xx and O3X1xx) please see the tagged releases 0.3.0 and 0.7.0 respectively. ::: -`ifm3d-ros2` is a wrapper around [ifm3d](https://github.com/ifm/ifm3d) enabling the usage of ifm O3R ToF camera platform from within [ROS 2](https://docs.ros.org/en/jazzy/index.html) software systems. +`ifm3d-ros2` is a wrapper around [ifm3d](https://github.com/ifm/ifm3d) enabling the usage of ifm O3R camera platform from within [ROS 2](https://index.ros.org/doc/ros2/) software systems. -![rviz](doc/figures/O3R_merged_point_cloud.png) +As of version 1.2.0, `ifm3d_ros2` also supports the Obstacle Detection Solution (ODS). Refer to [the ODS documentation](https://ifm3d.com/latest/ODS/index_ods.html) for more details on this application. + +![rviz](doc/camera_node/figures/O3R_merged_point_cloud.png) ## Software Compatibility Matrix -### Release versions +### Releases versions + +| `ifm3d_ros2` version | ifm3d version | O3R firmware version | ROS 2 distribution | Comment | +| -------------------- | -------------- | -------------------- | ------------------ | ---------------------------------- | +| 1.2.0 | 1.4.3 to 1.5.3 | 1.4.30 | Jazzy, Humble | Added support for ODS applications | +| 1.1.0 | 1.2.6 | 1.0.14 | Humble | | +| 1.0.1 | 0.93.0 | 0.14.23 | Foxy | | + +> Note: The version numbers listed above for the ifm3d API or the O3R firmware versions are the ones explicitly tested. Any other version might work but is not officially supported. -| ifm3d_ros2 version | ifm3d version | (O3R) embedded FW versions | ROS 2 distribution | -| ------------------ | ------------- | -------------------------- | ------------------ | -| 1.1.0 | 1.4.3 | 1.0.14, 1.1.x, 1.4.x | Humble | -| 1.0.1 | 0.93.0 | 0.14.23 | Foxy | +### Known Issues + +For a complete list of changes, refer to [the changelog](./CHANGELOG.rst). + +Below is a list of all known issues with the latest released version: +- There is an issue in how the intrinsic parameters are calculated: see [issue 18](https://github.com/ifm/ifm3d-ros2/issues/18). +- The `camera_info` topic does not work for rectification: see [issue 24](https://github.com/ifm/ifm3d-ros2/issues/24). + +### Deprecated ifm3d-ros2 versions -### Deprecated ifm3d-ros2 Versions The following versions are deprecated and no longer supported. -| ifm3d_ros2 version | ifm3d version | ROS 2 distribution | -| ------------------ | ------------- | ------------------ | -| 1.0.1 DEPRECATED | 0.93.0 | Galactic | -| 1.0.0 DEPRECATED | 0.92.0 | Galactic | -| 0.3.0 DEPRECATED | 0.17.0 | Dashing, Eloquent | -| 0.2.0 DEPRECATED | 0.12.0 | Dashing | -| 0.1.1 DEPRECATED | 0.12.0 | Dashing | -| 0.1.0 DEPRECATED | 0.12.0 | Dashing | - -## ToDo - -We are currently working on rounding out the feature set of our ROS2 interface. Our current objectives are to get the feature set to an equivalent -level to that of our ROS1 interface and to tune the ROS2/DDS performance to optimize the usage of our cameras from within ROS2 system (for different DDS implementations). -Thanks for your patience as we continue to ensure our ROS2 interface is feature-rich, robust, and performant. Your feedback is greatly appreciated. - -## Known Issues -+ Installing ifm3d API with it's default runtime libs may result in multiple versions of glog on the system: this results in a compilable but non-functional ROS node. -Please either use the glog version as included in your Ubuntu release (if compatible) or uninstall any incompatible lib version before installing the ifm3d required version. -We are working on providing a fix to the underlying API which removes the GLOG logging from the API. +| `ifm3d_ros2` version | ifm3d version | ROS 2 distribution | +| -------------------- | ------------- | ------------------ | +| 1.0.1 DEPRECATED | 0.93.0 | Galactic | +| 1.0.0 DEPRECATED | 0.92.0 | Galactic | +| 0.3.0 DEPRECATED | 0.17.0 | Dashing, Eloquent | +| 0.2.0 DEPRECATED | 0.12.0 | Dashing | +| 0.1.1 DEPRECATED | 0.12.0 | Dashing | +| 0.1.0 DEPRECATED | 0.12.0 | Dashing | + + ## LICENSE Please see the file called [LICENSE](LICENSE). diff --git a/build_container.sh b/build_container.sh index f8e7acb..45ee236 100755 --- a/build_container.sh +++ b/build_container.sh @@ -6,23 +6,23 @@ set -euo pipefail ############## ARCH="amd64" BASE_IMAGE="ros" -TAG=ifm3d-ros:humble-amd64 +TAG=ifm3d-ros:jazzy-amd64 ############## # For ARM64V8: ############## -# ARCH="arm64" -# BASE_IMAGE="arm64v8/ros" -# TAG=ifm3d-ros:humble-arm64_v8 +#ARCH="arm64" +#BASE_IMAGE="arm64v8/ros" +#TAG=ifm3d-ros:jazzy-arm64_v8 ############## # Arguments common for both architecture ############## -BUILD_IMAGE_TAG="humble" -FINAL_IMAGE_TAG="humble-ros-core" -IFM3D_VERSION="1.2.6" +BUILD_IMAGE_TAG="jazzy" +FINAL_IMAGE_TAG="jazzy-ros-core" +IFM3D_VERSION="1.5.3" IFM3D_ROS2_REPO="https://github.com/ifm/ifm3d-ros2.git" -IFM3D_ROS2_BRANCH="lm_humble_tests" +IFM3D_ROS2_BRANCH="v1.2.0" UBUNTU_VERSION="22.04" docker build -t $TAG \ diff --git a/config/camera_default_parameters.yaml b/config/camera_default_parameters.yaml index db6821d..4d33d17 100644 --- a/config/camera_default_parameters.yaml +++ b/config/camera_default_parameters.yaml @@ -5,7 +5,6 @@ ros__parameters: buffer_id_list: - CONFIDENCE_IMAGE - - DIAGNOSTIC - EXTRINSIC_CALIB - INTRINSIC_CALIB - INVERSE_INTRINSIC_CALIBRATION @@ -18,14 +17,10 @@ ip: 192.168.0.69 pcic_port: 50010 tf: - cloud_link: - frame_name: "camera_cloud_link" - publish_transform: true - mounting_link: - frame_name: "camera_mounting_link" - optical_link: - frame_name: "camera_optical_link" - publish_transform: true - transform: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + base_frame_name: "ifm_base_link" + mounting_frame_name: "camera_mounting_link" + optical_frame_name: "camera_optical_link" + publish_base_to_mounting: true + publish_mounting_to_optical: true use_sim_time: false xmlrpc_port: 80 diff --git a/config/examples/o3r_2d.yaml b/config/examples/o3r_2d.yaml index fb1c8fe..d975f2f 100644 --- a/config/examples/o3r_2d.yaml +++ b/config/examples/o3r_2d.yaml @@ -4,7 +4,6 @@ /ifm3d/camera_2d: ros__parameters: buffer_id_list: - - DIAGNOSTIC - JPEG_IMAGE - RGB_INFO frame_latency_thresh: 1.0 @@ -14,5 +13,11 @@ sync_clocks: false timeout_millis: 500 timeout_tolerance_secs: 5.0 + tf: + base_frame_name: "ifm_base_link" + mounting_frame_name: "camera_2d_mounting_link" + optical_frame_name: "camera_2d_optical_link" + publish_base_to_mounting: true + publish_mounting_to_optical: true use_sim_time: false xmlrpc_port: 80 diff --git a/config/examples/o3r_3d.yaml b/config/examples/o3r_3d.yaml index a9a80ec..4e919d7 100644 --- a/config/examples/o3r_3d.yaml +++ b/config/examples/o3r_3d.yaml @@ -5,10 +5,10 @@ ros__parameters: buffer_id_list: - CONFIDENCE_IMAGE - - DIAGNOSTIC - EXTRINSIC_CALIB - NORM_AMPLITUDE_IMAGE - RADIAL_DISTANCE_IMAGE + - TOF_INFO - XYZ frame_latency_thresh: 1.0 ip: 192.168.0.69 @@ -19,3 +19,9 @@ timeout_tolerance_secs: 5.0 use_sim_time: false xmlrpc_port: 80 + tf: + base_frame_name: "ifm_base_link" + mounting_frame_name: "camera_3d_mounting_link" + optical_frame_name: "camera_3d_optical_link" + publish_base_to_mounting: true + publish_mounting_to_optical: true diff --git a/config/examples/two_o3r_heads.yaml b/config/examples/two_o3r_heads.yaml index 3ab7003..1fff14f 100644 --- a/config/examples/two_o3r_heads.yaml +++ b/config/examples/two_o3r_heads.yaml @@ -5,7 +5,6 @@ /ifm3d/left_camera_2d: ros__parameters: buffer_id_list: - - DIAGNOSTIC - JPEG_IMAGE - RGB_INFO frame_latency_thresh: 1.0 @@ -15,6 +14,12 @@ sync_clocks: false timeout_millis: 500 timeout_tolerance_secs: 5.0 + tf: + base_frame_name: "ifm_base_link" + mounting_frame_name: "left_camera_2d_mounting_link" + optical_frame_name: "left_camera_2d_optical_link" + publish_base_to_mounting: true + publish_mounting_to_optical: true use_sim_time: false xmlrpc_port: 80 @@ -22,10 +27,10 @@ ros__parameters: buffer_id_list: - CONFIDENCE_IMAGE - - DIAGNOSTIC - EXTRINSIC_CALIB - NORM_AMPLITUDE_IMAGE - RADIAL_DISTANCE_IMAGE + - TOF_INFO - XYZ frame_latency_thresh: 1.0 ip: 192.168.0.69 @@ -34,13 +39,18 @@ sync_clocks: false timeout_millis: 500 timeout_tolerance_secs: 5.0 + tf: + base_frame_name: "ifm_base_link" + mounting_frame_name: "left_camera_3d_mounting_link" + optical_frame_name: "left_camera_3d_optical_link" + publish_base_to_mounting: true + publish_mounting_to_optical: true use_sim_time: false xmlrpc_port: 80 /ifm3d/right_camera_2d: ros__parameters: buffer_id_list: - - DIAGNOSTIC - JPEG_IMAGE - RGB_INFO frame_latency_thresh: 1.0 @@ -50,6 +60,12 @@ sync_clocks: false timeout_millis: 500 timeout_tolerance_secs: 5.0 + tf: + base_frame_name: "ifm_base_link" + mounting_frame_name: "right_camera_2d_mounting_link" + optical_frame_name: "right_camera_2d_optical_link" + publish_base_to_mounting: true + publish_mounting_to_optical: true use_sim_time: false xmlrpc_port: 80 @@ -57,17 +73,23 @@ ros__parameters: buffer_id_list: - CONFIDENCE_IMAGE - - DIAGNOSTIC - EXTRINSIC_CALIB - NORM_AMPLITUDE_IMAGE - RADIAL_DISTANCE_IMAGE + - TOF_INFO - XYZ frame_latency_thresh: 1.0 ip: 192.168.0.69 password: "" - pcic_port: 50013 # 3D stream of left camera connected to Port 3 + pcic_port: 50013 # 3D stream of right camera connected to Port 3 sync_clocks: false timeout_millis: 500 timeout_tolerance_secs: 5.0 + tf: + base_frame_name: "ifm_base_link" + mounting_frame_name: "right_camera_3d_mounting_link" + optical_frame_name: "right_camera_3d_optical_link" + publish_base_to_mounting: true + publish_mounting_to_optical: true use_sim_time: false xmlrpc_port: 80 diff --git a/config/ods_default_parameters.yaml b/config/ods_default_parameters.yaml new file mode 100644 index 0000000..12563ad --- /dev/null +++ b/config/ods_default_parameters.yaml @@ -0,0 +1,15 @@ +# Example configuration for an ODS application with two cameras. +# Expected to be used with ods.launch.py. + +# By default, this file expects two cameras with 3D ports connected to +# ports 2 and 3, and one ods application, "app0". + +/ifm3d/ods: + ros__parameters: + config_file: "config/examples/o3r_ods.json" + ip: 192.168.0.69 + pcic_port: 51010 + ods: + frame_id: "ifm_base_link" + publish_occupancy_grid: true + publish_costmap: false \ No newline at end of file diff --git a/config/param1.yml b/config/param1.yml deleted file mode 100644 index b87acde..0000000 --- a/config/param1.yml +++ /dev/null @@ -1,9 +0,0 @@ -################################################################################### -# # -# This parameter file is deprecated and will be removed in future distributions. # -# # -################################################################################### - -/ifm3d_ros2/camera1: - ros__parameters: - pcic_port: 50011 diff --git a/config/params.yaml b/config/params.yaml deleted file mode 100644 index 805c51a..0000000 --- a/config/params.yaml +++ /dev/null @@ -1,29 +0,0 @@ -################################################################################### -# # -# This parameter file is deprecated and will be removed in future distributions. # -# # -################################################################################### - -/ifm3d_ros2/camera0: - ros__parameters: - pcic_port: 50010 - -/ifm3d_ros2/camera1: - ros__parameters: - pcic_port: 50011 - -/ifm3d_ros2/camera2: - ros__parameters: - pcic_port: 50012 - -/ifm3d_ros2/camera3: - ros__parameters: - pcic_port: 50013 - -/ifm3d_ros2/camera4: - ros__parameters: - pcic_port: 50014 - -/ifm3d_ros2/camera5: - ros__parameters: - pcic_port: 50015 diff --git a/config/params_2cams.yaml b/config/params_2cams.yaml deleted file mode 100644 index 353a844..0000000 --- a/config/params_2cams.yaml +++ /dev/null @@ -1,13 +0,0 @@ -################################################################################### -# # -# This parameter file is deprecated and will be removed in future distributions. # -# # -################################################################################### - -/ifm3d_ros2/camera0: - ros__parameters: - pcic_port: 50010 - -/ifm3d_ros2/camera1: - ros__parameters: - pcic_port: 50012 diff --git a/doc/building.md b/doc/building.md index 979d5de..c5fba4f 100644 --- a/doc/building.md +++ b/doc/building.md @@ -3,7 +3,8 @@ ## Prerequisites ### Ubuntu and ROS -We suggest building the `ifm3d-ros2` node on top of [Ubuntu 22.04 Jammy Jellyfish](https://releases.ubuntu.com/jammy/) and [ROS Humble](https://docs.ros.org/en/humble/index.html). +We suggest building the `ifm3d-ros2` node on top of [Ubuntu 22.04 Jammy Jellyfish](https://releases.ubuntu.com/jammy/) and [ROS Humble](https://docs.ros.org/en/humble/index.html) or +[Ubuntu 24.04 Noble Numbat](https://releases.ubuntu.com/noble/) and [ROS Jazzy](https://docs.ros.org/en/jazzy/index.html) ### ifm3d C++ API The ROS node `ifm3d_ros2` requires the C++ API ifm3d to be installed locally for your system before compiling and running the ROS node. @@ -11,29 +12,18 @@ Refer to [the compatibility matrix](../README.md) to find out the correct ifm3d Follow these instructions on how to install `ifm3d` (we recommend using the pre-built package): [install ifm3d](https://api.ifm3d.com/stable/content/installation_instructions/install_linux_binary.html). -### Testing prerequisite - -These two packages are only required for testing but not at runtime: -- launch_testing -- launch_testing_ament_cmake - -On debian based systems they may be installed as follows: -``` -$ sudo apt install ros-${ROS_DISTRO}-launch-testing ros-${ROS_DISTRO}-launch-testing-ament-cmake -``` -:::{note} -The tests are currently a work in progress. -::: ## Build and install `ifm3d-ros2` -Building and installing ifm3d-ros2 is accomplished by utilizing the ROS2 [colcon](https://colcon.readthedocs.io/en/released/) tool. +Building and installing `ifm3d-ros2` is accomplished by utilizing the ROS 2 [colcon](https://colcon.readthedocs.io/en/released/) tool. ### Installation directory -First, we need to decide where we want our software to be installed. For purposes of this document, we will assume that we will install our ROS packages at `~/colcon_ws/src`. +First, we need to decide where we want our software to be installed. +For purposes of this document, we will assume that we will install our ROS packages at `~/colcon_ws/`. :::{note} Below we assume `humble`. Adapting to other ROS distributions is left as an exercise for the reader. ::: + ### Colcon workspace Next, we want to create a _colcon workspace_ that we can use to build and install that code from. @@ -42,9 +32,11 @@ $ mkdir -p ~/colcon_ws/src ``` ### Get the `ifm3d-ros2` code from GitHub -Next, we need to get the code from GitHub. Please adapt the commands when not following the suggested directory structure: `~/colcon_ws/src/` + +Next, we need to get the code from GitHub. Please adapt the commands when not following the suggested directory structure. ```bash +$ source /opt/ros/humble/setup.bash $ cd ~/colcon_ws/src $ git clone https://github.com/ifm/ifm3d-ros2.git $ git checkout # Replace the targeted version @@ -61,7 +53,8 @@ Build your workspace: ```bash $ cd ~/colcon_ws/ -$ colcon build --cmake-args -DBUILD_TESTING=OFF +$ rosdep install --from-paths src --ignore-src -r -y +$ colcon build Starting >>> ifm3d_ros2 Finished <<< ifm3d_ros2 [17.6s] @@ -72,9 +65,9 @@ Summary: 1 package finished [17.8s] The tests are not functional at the moment. ::: -To confirm that the node is functional, try [launching it](../doc/launch.md) and inspecting [the published topics](../doc/topics.md). +To confirm that the node is functional, try [launching it](camera_node/launch.md) and inspecting [the published topics](camera_node/topics.md). ## Docker containers We provide a Dockerfile build instruction for building the ROS2 node inside a Docker container. -For reference and for deployment of the ROS node via a Docker software Docker container see the [Dockerfile](../Dockerfile). \ No newline at end of file +For reference and for deployment of the ROS node via a Docker container see the [Dockerfile](../Dockerfile). \ No newline at end of file diff --git a/doc/camera_node/camera_transforms.md b/doc/camera_node/camera_transforms.md new file mode 100644 index 0000000..e69de29 diff --git a/doc/figures/O3R_merged_point_cloud.png b/doc/camera_node/figures/O3R_merged_point_cloud.png similarity index 100% rename from doc/figures/O3R_merged_point_cloud.png rename to doc/camera_node/figures/O3R_merged_point_cloud.png diff --git a/doc/figures/rviz_sample.png b/doc/camera_node/figures/rviz_sample.png similarity index 100% rename from doc/figures/rviz_sample.png rename to doc/camera_node/figures/rviz_sample.png diff --git a/doc/camera_node/figures/transforms-1.png b/doc/camera_node/figures/transforms-1.png new file mode 100644 index 0000000..947082f Binary files /dev/null and b/doc/camera_node/figures/transforms-1.png differ diff --git a/doc/camera_node/figures/transforms-2.png b/doc/camera_node/figures/transforms-2.png new file mode 100644 index 0000000..8723fed Binary files /dev/null and b/doc/camera_node/figures/transforms-2.png differ diff --git a/doc/camera_node/index_camera_node.md b/doc/camera_node/index_camera_node.md new file mode 100644 index 0000000..9a9d1b2 --- /dev/null +++ b/doc/camera_node/index_camera_node.md @@ -0,0 +1,11 @@ +# Camera node + +The `ifm3d-ros2` package provides a camera node, which is the main interface for getting RGB or 3D data, and configuring an O3R platform. + +:::{toctree} + :maxdepth: 2 +Launch +Topics +Parameters +Multi-camera applications +::: \ No newline at end of file diff --git a/doc/launch.md b/doc/camera_node/launch.md similarity index 85% rename from doc/launch.md rename to doc/camera_node/launch.md index a49c3fa..3ad7862 100644 --- a/doc/launch.md +++ b/doc/camera_node/launch.md @@ -2,14 +2,17 @@ Launch the camera node (assuming you are in `~/colcon_ws/`): ``` -$ . install/setup.bash +$ source install/setup.bash $ ros2 launch ifm3d_ros2 camera.launch.py ``` -This will launch a `/ifm3d/camera/` node with default arguments, i.e. utilize the defaults yml configuration `camera_default_parameters.yaml`. +This will launch the `/ifm3d/camera/` node with default arguments, using the default YAML configuration file `camera_default_parameters.yaml`. Depending on the specified `pcic_port`, the node will initialize either a 3D camera (if the port corresponds to a 3D camera) or a 2D camera. + +The respective node information should look like this: -The respective node information should look like this: `ros2 node info /ifm3d/camera` ``` +$ ros2 node info /ifm3d/camera + /ifm3d/camera Subscribers: /parameter_events: rcl_interfaces/msg/ParameterEvent @@ -64,6 +67,6 @@ $ ros2 launch ifm3d_ros2 camera.launch.py visualization:=true ``` -![rviz1](figures/O3R_merged_point_cloud.png) +![rviz1](./figures/O3R_merged_point_cloud.png) Congratulations! You can now have complete control over the O3R perception platform from inside ROS2. diff --git a/doc/camera_node/multi_head.md b/doc/camera_node/multi_head.md new file mode 100644 index 0000000..97ddff8 --- /dev/null +++ b/doc/camera_node/multi_head.md @@ -0,0 +1,64 @@ +# Multi head launch file configuration + +It is possible to stream data from multiple cameras or from the two ports of one camera at once. +This can be achieved by setting the camera-specific parameters in the launch configuration files, namely `pcic-port` and `buffer_id_lists`. +We provide examples of default parameters for getting the 2D or 3D data from one camera, or all the data from two full O3R camera (two 2D data streams and two 3D data streams). + +## `o3r_2d.yaml` +This example configuration connects to the 2D (RGB) data stream of one O3R camera head connected to port 0: + +The ports on the VPU should be connected as follows: +* Camera 2D: physical port 0 - corresponds to `pcic_port=50010` + +To launch this example, use the following command: +```bash +ros2 launch ifm3d_ros2 camera.launch.py camera_name:=camera_2d parameter_file_name:=examples/o3r_2d.yaml +``` +> **Note:** Don’t forget to specify the `camera_name` parameter if it differs from the default value, "camera", and the `camera_namespace` if it differs from the default, "ifm3d". + +## `o3r_3d.yaml` +This example configuration connects to the 3D data stream of one O3R camera head connected to port 2: + +The ports on the VPU should be connected as follows: +* Camera 3D: physical port 2 - corresponds to `pcic_port=50012` + +To launch this example, use the following command: +```bash +ros2 launch ifm3d_ros2 camera.launch.py camera_name:=camera_3d parameter_file_name:=examples/o3r_3d.yaml +``` +> **Note:** Don’t forget to specify the `camera_name` parameter if it differs from the default value, "camera", and the `camera_namespace` if it differs from the default, "ifm3d". + +## Launching a 2D and a 3D node simultaneously +This example configuration connects to one 3D data stream **AND** one 2D (RGB) data stream connected to ports 0 and 2: + +- Camera 2D: physical port 0 - corresponds to `pcic_port=50010` +- Camera 3D: physical port 2 - corresponds to `pcic_port=50012` + +To launch this example, use the following command: +```bash +ros2 launch ifm3d_ros2 example_o3r_2d_and_3d.launch.py +``` + + +## `two_o3r_heads.yaml` +This example configuration connects to two 3D data stream **AND** two 2D (RGB) data stream of **two** O3R camera head connected to ports 0 - 3: + +The ports on the VPU should be connected as follows: +Camera head 1: +* Camera 2D: physical port 0 - corresponds to `pcic_port=50010` +* Camera 3D: physical port 2 - corresponds to `pcic_port=50012` + +Camera head 2: +* Camera 2D: physical port 1 - corresponds to `pcic_port=50011` +* Camera 3D: physical port 3 - corresponds to `pcic_port=50013` + +To launch this example, use the following command: +```bash +ros2 launch ifm3d_ros2 example_two_o3r_heads.launch.py parameter_file_name:=two_o3r_heads.yaml +``` + +## Adapting the camera configuration to your needs +The example configurations are provided in the directory `config/examples`. +They can act as inspiration when configuring your own multi-camera setup. + +To adjust the configuration to your own setup, use these configuration files as a base to add or remove cameras, change the port number or edit the list of requested buffers. \ No newline at end of file diff --git a/doc/camera_node/parameters.md b/doc/camera_node/parameters.md new file mode 100644 index 0000000..aa71073 --- /dev/null +++ b/doc/camera_node/parameters.md @@ -0,0 +1,40 @@ +# Parameters + +| Name | Data Type | Default Value | Description | +| ---------------------------------- | ------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------ | +| `~/buffer_id_list` | string array | `{"CONFIDENCE_IMAGE", "EXTRINSIC_CALIB", "NORM_AMPLITUDE_IMAGE", "RADIAL_DISTANCE_IMAGE", "TOF_INFO", "XYZ"}` for the TOF cameras and `{"JPEG_IMAGE", "RGB_INFO"}` for the RGB cameras. | List of buffer_id strings denoting the wanted buffers. | +| `~/config_file` | string | `""` | Path to a JSON configuration file to be used when configuring the node. | +| `~/ip` | string | 192.168.0.69 | The ip address of the camera. | +| `~/log_level` | string | warning | ifm3d-ros2 node logging level. | +| `~/pcic_port` | int | 50010 | PCIC port corresponding to the targeted camera port. | +| `~/tf.base_frame_name` | string | `ifm_base_link` | Name for ifm reference frame. | +| `~/tf.mounting_frame_name` | string | `_mounting_link` | Name for the mounting point frame. | +| `~/tf.optical_frame_name` | string | `_optical_link` | Name for the optical frame. | +| `~/tf.publish_base_to_mounting` | bool | true | Whether the transform from the ifm base link to the camera mounting point should be published. | +| `~/tf.publish_mounting_to_optical` | bool | true | Whether the transform from the cameras mounting point to the optical center should be published. | +| `~/xmlrpc_port` | uint | 50010 | TCP port the on-camera xmlrpc server is listening on. | + +## Details on the published transforms + +The `ifm3d-ros2` node can publish two transforms: +* one from the `ifm_base_link` to the `mounting_link`, according to the camera calibration (either via JSON or via the ifm Vision Assistant GUI), +* and one from the `mounting_link` the `optical_link`, according to the manufacturing parameters. + +The publication of these transform can be deactivated via parameter. +The names for all three links can be changed via node parameters. + +To clarify which frame each of these transform refers to, consider the drawings below: + +![Description of the base, mounting and optical frames](./figures/transforms-1.png) + + +The reference of the mounting frame is at the back of the O3R camera head housing (scale drawings can be found on ifm.com in the download section of the specific article). +The reference for the ifm base frame is defined by the extrinsic parameters set in the JSON configuration of the O3R platform (`extrinsicHeadToUser`). +Generally, the ifm base frame is configured to have for origin the center of the robot coordinate system, but this is not required. +When no extrinsic parameter is set, the ifm base frame and the mounting frame are the same. + +![Focused description of the optical and mounting frames](./figures/transforms-2.png) + +The optical frame refers to the reference point of the optical system (lens, chip, etc). +A static transform is published between the mounting link and the optical link, that corresponds to the intrinsic calibration parameters of the camera. +Each set of intrinsic parameters is unique to a specific camera head and set in production. These parameters are not expected to change over time. \ No newline at end of file diff --git a/doc/camera_node/topics.md b/doc/camera_node/topics.md new file mode 100644 index 0000000..f8baa4e --- /dev/null +++ b/doc/camera_node/topics.md @@ -0,0 +1,26 @@ +# Topics + +## Published Topics + +| Name | Data Type | Description | +| ------------------------------- | --------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------- | +| `amplitude` | `sensor_msgs/msg/Image` | The normalized amplitude image | +| `cloud` | `sensor_msgs/msg/PointCloud2` | The point cloud data | +| `confidence` | `sensor_msgs/msg/Image` | The confidence image | +| `distance` | `sensor_msgs/msg/Image` | The radial distance image | +| `rgb` | `sensor_msgs/msg/Image` | The RGB 2D image of the 2D imager | +| `extrinsics` | `ifm3d_ros2::msg::Extrinsics` | The extrinsic calibration of the camera (camera to world) | +| `intrinsic_calib` | `ifm3d_ros2::msg::Intrinsics` | The intrinsic calibration of the camera (optical system parameters) | +| `inverse_intrinsic_calibration` | `ifm3d_ros2::msg::InverseIntrinsics` | The inverse intrinsic calibration of the camera | +| `camera_info` | `sensor_msgs::msg::CameraInfo` | The camera info topic containing the distortion model. This topic is published if the `INTRINSIC_CALIB` is part of the buffer list for the 3D cameras, or if the `RGB_INFO` is part of the buffer list for the RGB cameras. | +| `tof_info` | `ifm3d_ros2::msg::TOFInfo` | A topic gathering various information from the tof camera (see [TOFinfo.msg](../msg/TOFInfo.msg)) | +| `rgb_info` | `ifm3d_ros2::msg::RGBInfo` | A topic gathering various information from the rgb camera (see [RGBInfo.msg](../msg/RGBInfo.msg)) | +| `diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic messages pulled from the device every second | + +:::{note} +All the topics are published with QoS `ifm3d_ros2::LowLatencyQoS`. +::: + +## Subscribed Topics + +None. diff --git a/doc/deployment.md b/doc/deployment.md index b4a7c1f..6c406ed 100644 --- a/doc/deployment.md +++ b/doc/deployment.md @@ -4,11 +4,13 @@ Follow these steps to get our supplied Docker container to run on your system. :::{note} The instructions below apply to ROS2 Humble. Please change the commands to suit your ROS 2 distribution. ::: + ## Build the docker container -We provide a Dockerfile and a build script to help you build a docker container with ifm3d and ifm3d-ros2. To use it, check out the `build_container.sh` script. Open it up and adjust the arguments to suit your target architecture (arm64v8 or amd64) and the targeted ifm3d and ifm3d-ros2 version. Then you can build the container: + +We provide a Dockerfile and a build script to help you build a docker container with `ifm3d` and `ifm3d-ros2`. To get started, check out the [`build_container.sh`](../build_container.sh) script. Open it up and adjust the arguments to suit your target architecture (arm64v8 or amd64) and the targeted `ifm3d` and `ifm3d-ros2` version. Then you can build the container: ```bash -$ ./build_humble.sh +$ ./build_container.sh [+] Building 675.3s (23/23) FINISHED => [internal] load build definition from Dockerfile 0.0s => => transferring dockerfile: 2.92kB 0.0s @@ -46,6 +48,7 @@ To deploy the container on to the VPU, or use the container locally to interact Resources on the OVP8xx are limited and shared between all the running processes. We recommend assigning the Docker process to specific cores so as not to interfere with other applications. Refer to [the resource management documentation on ifm3d.com](https://ifm3d.com/latest/SoftwareInterfaces/Docker/cpu.html). ## Distributed setup + It is possible to run a complete ROS system in a distributed way. In this section we provide instructions to run ifm3d-ros2 in a container deployed on the VPU (primary container), and the visualization locally on a laptop (secondary system). These instructions can be adapted to suit other architectural designs. @@ -55,7 +58,9 @@ These instructions can be adapted to suit other architectural designs. ```bash $ docker save docker.io/library/ifm3d-ros:humble-arm64_v8 | ssh -C oem@192.168.0.69 docker load ``` + - SSH to the VPU and run the container: +- ```bash $ ssh oem@192.168.0.69 #Adapt to the IP address of your VPU o3r-vpu-c0$ docker run -ti --net=host ifm3d-ros:humble-arm64_v8 @@ -82,7 +87,7 @@ root@62b0c2e120bb:/home/ifm/$ ros2 launch ifm3d_ros2 camera.launch.py parameter 1. Source ROS2 on the secondary machine: ```bash -$ source /opt/ros/galactic/setup.bash +$ source /opt/ros/humble/setup.bash ``` - Check that ROS topics are available (on `ROS_DOMAIN_ID=0`): ```bash diff --git a/doc/diagnostic.md b/doc/diagnostic.md new file mode 100644 index 0000000..e3eba9d --- /dev/null +++ b/doc/diagnostic.md @@ -0,0 +1,32 @@ +# Diagnostic + +Both the camera node and the ODS node publish diagnostic information to the `/diagnostic` topic. +The diagnostic message contains an error code and a message, referring directly to an error from ifm3d or from the embedded software. + +Below is an example of a diagnostic message: +```bash +$ ros2 topic echo /diagnostics +header: + stamp: + sec: 1652509745 + nanosec: 818232736 + frame_id: '' +status: +- level: "\x02" + name: '' + message: '' + hardware_id: /ifm3d/diag_module + values: + - key: bootid + value: '"71fd8e7d-385a-4f86-8a88-bb59f5112c73"' + - key: events + value: '[{"description":"Unable to determine velocity","id":105007,"name":"ERROR_ODSAPP_VELOCITY_UNAVAILABLE","source":"/applications/in...' + - key: timestamp + value: '1652509745818232736' + - key: version + value: '{"diagnostics":"0.0.11","euphrates":"1.34.226","firmware":"1.1.41.4507"}' +--- +``` + +For more details on the error codes and potential troubleshooting strategies, refer to the [O3R diagnostic documentation](../../../documentation/SoftwareInterfaces/ifmDiagnostic/index_diagnostic.md). + diff --git a/doc/figures/transforms-1.png b/doc/figures/transforms-1.png deleted file mode 100644 index f3278ab..0000000 Binary files a/doc/figures/transforms-1.png and /dev/null differ diff --git a/doc/figures/transforms-2.png b/doc/figures/transforms-2.png deleted file mode 100644 index 30a5460..0000000 Binary files a/doc/figures/transforms-2.png and /dev/null differ diff --git a/doc/multi_head.md b/doc/multi_head.md deleted file mode 100644 index ef551d8..0000000 --- a/doc/multi_head.md +++ /dev/null @@ -1,52 +0,0 @@ -# Multi head launch file configuration - -It is possible to stream data from multiple imagers and camera heads (i.e. ports): while connecting both 2D and 3D data from your camera with your VPU or even with multiple cameras connected to one VPU. - -This can be achieved by altering your launch configuration yaml file and setting the camera-specific parameters, namely `pcic-port` and `buff_id_lists`, correctly. -There are examples provided doing exactly that. - -## `o3r_2d.yaml` -This example configuration connects to the 2D (RGB) data stream of one O3R camera head connected to port 0: - -The Ports on the VPU should be connected as follows: -* Camera 2D: physical port 0 - corresponds to `pcic_port=50010` - -To launch this example, use the following command: -```bash -ros2 launch ifm3d_ros2 camera.launch.py parameter_file_name:=o3r_2d.yaml -``` - -## `o3r_3d.yaml` -This example configuration connects to the 3D (TOF) data stream of one O3R camera head connected to port 2: - -The Ports on the VPU should be connected as follows: -* Camera 3D: physical port 2 - corresponds to `pcic_port=50012` - -To launch this example, use the following command: -```bash -ros2 launch ifm3d_ros2 camera.launch.py parameter_file_name:=o3r_3d.yaml -``` - -## `two_o3r_heads.yaml` -This example configuration connects to two 3D (TOF) data stream **AND** two 2D (RGB) data stream of **two** O3R camera head connected to ports 0 - 3: - -The Ports on the VPU should be connected as follows: -Camera head 1: -* Camera 2D: physical port 0 - corresponds to `pcic_port=50010` -* Camera 3D: physical port 2 - corresponds to `pcic_port=50012` - -Camera head 2: -* Camera 2D: physical port 1 - corresponds to `pcic_port=50011` -* Camera 3D: physical port 3 - corresponds to `pcic_port=50013` - -To launch this example, use the following command: -```bash -ros2 launch ifm3d_ros2 two_o3r_heads.launch.py parameter_file_name:=two_o3r_heads.yaml -``` - -### Adapting the camera configuration to your needs -The example configurations are provided in the directory `config/examples`. -They can act as inspiration when configuring your own multi-camera setup. - -Please edit / add your own specific hardware and software configuration via these configuration yml files. -For a specific number of ros node camera streams other than 1 or 4 (see examples) please create a updated launch helper yourself. \ No newline at end of file diff --git a/doc/ods_node/index_ods_node.md b/doc/ods_node/index_ods_node.md new file mode 100644 index 0000000..61e7cd2 --- /dev/null +++ b/doc/ods_node/index_ods_node.md @@ -0,0 +1,12 @@ +# ODS node + +The `ifm3d-ros2` package provides an Obstacle Detection Solution (ODS) node, which is the main interface for streaming occupancy grid and zone data from an ODS application on the O3R platform, and configuring it. + +:::{toctree} + :maxdepth: 2 + ODS configuration + Launch + Topics + Transforms + Parameters +::: \ No newline at end of file diff --git a/doc/ods_node/ods_configuration.md b/doc/ods_node/ods_configuration.md new file mode 100644 index 0000000..ca8ebd5 --- /dev/null +++ b/doc/ods_node/ods_configuration.md @@ -0,0 +1,38 @@ +# Configuring ODS + +:::{note} +ifm's Obstacle Detection Solution (ODS) is supported in `ifm3d-ros2` version 1.2.0 and above. +::: + +ODS is an application that runs on the O3R platform, that provides the ability to detect the position of obstacles within the camera field of view. +For more details on ODS, refer to [the ODS documentation on ifm3d.com](https://ifm3d.com/latest/ODS/index_ods.html). + +## Using the Vision Assistant + +There are multiple ways to create and configure an ODS application. +We typically recommend to use the ifm Vision Assistant, which is ifm's official GUI for interfacing with all the vision products. + +To get started with ODS in the Vision Assistant, refer to [the ODS getting started documentation](https://ifm3d.com/latest/ODS/getting_started.html). +After following these instructions, you will have set up and properly configured an ODS application, and you will be ready to launch the ODS node. + +## Using the `Config` service + +It is also possible to configure an application directly using the `ifm3d-ros2` service `Config`. +We recommend to use this method when re-configuring the application at runtime, for example to change the active ports. + +In this case, you can use the following command. +This will switch the `activePort` parameter of the application `app0` to `"port3"`. +```bash +$ ros2 service call /ifm3d/ods/Config ifm3d_ros2/srv/Config "{json: '{\"applications\":{\"instances\":{\"app0\":{\"configuration\":{\"activePorts\":[\"port3\"]}}}}}'}" +requester: making request: ifm3d_ros2.srv.Config_Request(json='{"applications":{"instances":{"app0":{"configuration":{"activePorts":["port3"]}}}}}') + +response: +ifm3d_ros2.srv.Config_Response(status=0, msg='OK') + +``` + +## Using the `config_file` parameter + +Additionally, it is possible to configure an application (or any other aspect of the system) using a configuration file, provided through the `config_file` parameter. +This configuration file will be used to set the configuration in the `on_configuration` transition of the node. +To re-configure a node using a new configuration file, the `on_configuration` transition has to be triggered again. \ No newline at end of file diff --git a/doc/ods_node/ods_launch.md b/doc/ods_node/ods_launch.md new file mode 100644 index 0000000..4a15a5f --- /dev/null +++ b/doc/ods_node/ods_launch.md @@ -0,0 +1,88 @@ +# Launch the ODS node + +To launch the ODS node, you can use the provided launchfile `ods.launch.py`: +```bash +$ ros2 launch ifm3d_ros2 ods.launch.py +[INFO] [launch]: Default logging verbosity is set to INFO +[INFO] [ods_standalone-1]: process details: cmd='/home/usmasslo/ROS/ROS2/colcon_ws/install/ifm3d_ros2/lib/ifm3d_ros2/ods_standalone --ros-args --log-level info --ros-args -r __node:=ods -r __ns:=/ifm3d --params- +file ~/colcon_ws/install/ifm3d_ros2/share/ifm3d_ros2/config/ods_default_parameters.yaml', cwd='None', custom_env?=True +[INFO] [ods_standalone-1]: process started with pid [486398] +[ods_standalone-1] [INFO] [1728931867.359101479] [ifm3d.ods]: namespace: /ifm3d +[ods_standalone-1] [INFO] [1728931867.359179144] [ifm3d.ods]: node name: ods +[ods_standalone-1] [INFO] [1728931867.359186657] [ifm3d.ods]: middleware: rmw_fastrtps_cpp +[ods_standalone-1] [INFO] [1728931867.359190490] [ifm3d.ods]: Declaring parameters... +[ods_standalone-1] [INFO] [1728931867.359323428] [ifm3d.ods]: After the parameters declaration +[ods_standalone-1] [INFO] [1728931867.359406942] [ifm3d.ods]: node created, waiting for `configure()`... +[ods_standalone-1] [INFO] [1728931867.521751135] [ifm3d.ods]: on_configure(): unconfigured -> configuring +[ods_standalone-1] [INFO] [1728931867.521862570] [ifm3d.ods]: Parsing parameters... [ods_standalone-1] [INFO] [1728931867.521944451] [ifm3d.ods]: ip: 192.168.0.69 +[ods_standalone-1] [INFO] [1728931867.521980451] [ifm3d.ods]: pcic_port: 51010 +[ods_standalone-1] [INFO] [1728931867.521997881] [ifm3d.ods]: xmlrpc_port: 80 +[ods_standalone-1] [INFO] [1728931867.522012697] [ifm3d.ods]: Parameters parsed. +[ods_standalone-1] [INFO] [1728931867.522028686] [ifm3d.ods]: Adding callbacks to handle parameter changes at runtime... +[ods_standalone-1] [INFO] [1728931867.525109429] [ifm3d.ods]: Callbacks set. +[ods_standalone-1] [INFO] [1728931867.525177112] [ifm3d.ods]: Initializing Device +[ods_standalone-1] [INFO] [1728931867.525325450] [ifm3d.ods]: Initializing FrameGrabber for data +[ods_standalone-1] [INFO] [1728931868.040227637] [ifm3d.ods]: Creating OdsModule... +[ods_standalone-1] [INFO] [1728931868.040519950] [ifm3d.ods]: FunctionModule contructor called. +[ods_standalone-1] [INFO] [1728931868.040545387] [ifm3d.ods]: OdsModule contructor called. +[ods_standalone-1] [INFO] [1728931868.040638930] [ifm3d.ods]: OdsModule created. +[ods_standalone-1] [INFO] [1728931868.040644930] [ifm3d.ods]: Creating DiagModule... +[ods_standalone-1] [INFO] [1728931868.040675690] [ifm3d.ods]: FunctionModule contructor called. +[ods_standalone-1] [INFO] [1728931868.040736808] [ifm3d.ods]: hardware_id: /ifm3d/diag_module +[ods_standalone-1] [INFO] [1728931868.040756934] [ifm3d.ods]: DiagModule created. +[ods_standalone-1] [INFO] [1728931868.040803035] [ifm3d.ods]: OdsModule: on_configure called +[ods_standalone-1] [INFO] [1728931868.055527902] [ifm3d.ods]: Parameter ods.frame_id set to 'ods_frame' +[ods_standalone-1] [INFO] [1728931868.055542337] [ifm3d.ods]: DiagModule: on_configure called +[ods_standalone-1] [INFO] [1728931868.057683191] [ifm3d.ods]: Creating BaseServices... +[2] +[ods_standalone-1] [INFO] [1728931868.057733035] [ifm3d.ods]: BaseServices constructor called. +[ods_standalone-1] [INFO] [1728931868.060201097] [ifm3d.ods]: Services created; +[ods_standalone-1] [INFO] [1728931868.060224508] [ifm3d.ods]: BaseServices created. +[ods_standalone-1] [INFO] [1728931868.060237899] [ifm3d.ods]: Configuration complete. +[ods_standalone-1] [INFO] [1728931868.061711296] [ifm3d.ods]: on_activate(): inactive -> activating [89/139] +[ods_standalone-1] [INFO] [1728931868.061807873] [ifm3d.ods]: Starting the Framegrabbers... +[ods_standalone-1] [INFO] [1728931868.187758863] [ifm3d.ods]: Diagnostic monitoring active. +[ods_standalone-1] [INFO] [1728931868.187814195] [ifm3d.ods]: OdsModule: on_activate called +[ods_standalone-1] [INFO] [1728931868.187832369] [ifm3d.ods]: DiagModule: on_activate called +``` + +This will launch the `/ifm3d/ods` node, using the default parameters defined in `config/ods_default_parameters.yaml`. + +This node provides the following interfaces: +```bash +$ ros2 node info /ifm3d/ods +/ifm3d/ods + Subscribers: + /parameter_events: rcl_interfaces/msg/ParameterEvent + Publishers: + /diagnostics: diagnostic_msgs/msg/DiagnosticArray + /ifm3d/ods/ods_info: ifm3d_ros2/msg/Zones + /ifm3d/ods/ods_occupancy_map_ifm: ifm3d_ros2/msg/OccGrid + /ifm3d/ods/ods_occupancy_map_ros: nav_msgs/msg/OccupancyGrid + /ifm3d/ods/transition_event: lifecycle_msgs/msg/TransitionEvent + /parameter_events: rcl_interfaces/msg/ParameterEvent + /rosout: rcl_interfaces/msg/Log + Service Servers: + /ifm3d/ods/Config: ifm3d_ros2/srv/Config + /ifm3d/ods/Dump: ifm3d_ros2/srv/Dump + /ifm3d/ods/GetDiag: ifm3d_ros2/srv/GetDiag + /ifm3d/ods/Softoff: ifm3d_ros2/srv/Softoff + /ifm3d/ods/Softon: ifm3d_ros2/srv/Softon + /ifm3d/ods/change_state: lifecycle_msgs/srv/ChangeState + /ifm3d/ods/describe_parameters: rcl_interfaces/srv/DescribeParameters + /ifm3d/ods/get_available_states: lifecycle_msgs/srv/GetAvailableStates + /ifm3d/ods/get_available_transitions: lifecycle_msgs/srv/GetAvailableTransitions + /ifm3d/ods/get_parameter_types: rcl_interfaces/srv/GetParameterTypes + /ifm3d/ods/get_parameters: rcl_interfaces/srv/GetParameters + /ifm3d/ods/get_state: lifecycle_msgs/srv/GetState + /ifm3d/ods/get_transition_graph: lifecycle_msgs/srv/GetAvailableTransitions + /ifm3d/ods/get_type_description: type_description_interfaces/srv/GetTypeDescription + /ifm3d/ods/list_parameters: rcl_interfaces/srv/ListParameters + /ifm3d/ods/set_parameters: rcl_interfaces/srv/SetParameters + /ifm3d/ods/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically + Service Clients: + + Action Servers: + + Action Clients: +``` \ No newline at end of file diff --git a/doc/ods_node/ods_params.md b/doc/ods_node/ods_params.md new file mode 100644 index 0000000..28aa258 --- /dev/null +++ b/doc/ods_node/ods_params.md @@ -0,0 +1,11 @@ +# ODS parameters + +| Name | Data Type | Default Value | Description | +| ------------------------------ | --------- | ---------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `~/config_file` | string | `""` | Path to a JSON configuration file to be used when configuring the node. | +| `~/ip` | String | "192.168.0.69" | The IP address of the OVP8xx platform. | +| `~/ods.frame_id` | String | "ifm_base_link" | The name of the `frame_id` used for the occupancy grid and the zones topics headers. | +| `~/ods.publish_occupancy_grid` | bool | true | Set module to publish `nav2_msgs/OccupancyGrid`. | +| `~/ods.publish_costmap` | bool | false | Set module to publish `nav2_msgs/Costmap`. | +| `~/pcic_port` | Integer | 51010 | The TCP port the PCIC server for the active application is listening to. Can be read out in the JSON configuration at the `"/applications/instances/appX/data/PcicTCPPort"` key, or retrieved using the ifm3d API with `O3R->Port("appX").pcic_port`, where `"appX"` is the active application. | +| `~/xmlrpc_port` | Integer | `ifm3d::DEFAULT_XMLRPC_PORT` | The TCP port the XMLRPC server for the active application is listening to. Typically, the default value can be used. | diff --git a/doc/ods_node/ods_topics.md b/doc/ods_node/ods_topics.md new file mode 100644 index 0000000..366e7a6 --- /dev/null +++ b/doc/ods_node/ods_topics.md @@ -0,0 +1,16 @@ +# ODS topics + +## Published topics +| Name | Data type | Description | +| ---------------------------------- | ---------------------------- | -------------------------------------------------------------------------------------------------------------------------------- | +| `/ifm3d/ods/ods_info` | `ifm3d_ros2/msg/Zones` | Provides the id of the currently active zone set, as well as whether each zone is occupied (`=1`) or not (`=0`). | +| `/ifm3d/ods/ods_occupancy_grid` | `nav_msgs/msg/OccupancyGrid` | The occupancy grid where each cell represent the probability of an obstacle being there. | +| `/ifm3d/ods/ods_occupancy_grid_costmap` | `nav2_msgs/msg/Costmap` | The occupancy grid, formatted as a costmap. Publication of this topic is optional. | +| `/diagnostics` | `diagnostic_msgs/msg/DiagnosticArray` | Check out [the diagnostic documentation](../diagnostic.md) for more details. | + +:::{note} +All the topics are published with QoS `ifm3d_ros2::LowLatencyQoS`. +::: + +## Subscribed topics +None \ No newline at end of file diff --git a/doc/ods_node/ods_transforms.md b/doc/ods_node/ods_transforms.md new file mode 100644 index 0000000..6ccd5cf --- /dev/null +++ b/doc/ods_node/ods_transforms.md @@ -0,0 +1,14 @@ +# ODS transforms + +ODS data is published with reference to the `"ifm_base_link"`, which, in general, corresponds to the robot's `"base_link"`. + +The reference coordinate system for ODS is constrained such that: +- The center of the ODS coordinate system is at floor level, +- The X axis is pointing in the direction of forward movement, +- The Z axis is pointing upwards. + +The cameras used by ODS must be calibrated with reference to the ODS coordinate system, as part of the initial ODS configuration (see [the ODS configuration documentation](./ods_configuration.md)). + +The origin of the ODS coordinate system corresponds to the center of the occupancy grid. + +Publishing the tf from the robot's `"base_link"` to the `"ifm_base_link"` should be done by the user. diff --git a/doc/parameters.md b/doc/parameters.md deleted file mode 100644 index 1d7d08d..0000000 --- a/doc/parameters.md +++ /dev/null @@ -1,29 +0,0 @@ -# Parameters -| Name | Data Type | Default Value | Description | -| ------------------------ | --------- | ------------- | ------------ | -| ~/buffer_id_list | string array | {"CONFIDENCE_IMAGE","EXTRINSIC_CALIB","JPEG_IMAGE" "NORM_AMPLITUDE_IMAGE","RADIAL_DISTANCE_IMAGE","RGB_INFO","XYZ"}| List of buffer_id strings denoting the wanted buffers.| -| ~/ip | string | 192.168.0.69 | The ip address of the camera. | -| ~/log_level| string | warning| ifm3d-ros2 node logging level. | -| ~/tf.cloud_link.frame_name | string | _optical_link | Name for the point cloud frame. | -| ~/tf.cloud_link.publish_transform | bool | true | Whether the transform from the cameras mounting point to the point cloud center should be published. | -| ~/tf.mounting_link.frame_name | string | _mounting_link | Name for the mounting point frame. | -| ~/tf.optical_link.frame_name | string | _optical_link | Name for the point optical frame. | -| ~/tf.optical_link.publish_transform | bool | true | Whether the transform from the cameras mounting point to the point optical center should be published. | -| ~/tf.optical_link.transform | double array | [0, 0, 0, 0, 0, 0] | Static transform from mounting link to optical link, as [x, y, z, rot_x, rot_y, rot_z] | -| ~/buffer_id_list | string array | {"NORM_AMPLITUDE_IMAGE", "CONFIDENCE_IMAGE", JPEG_IMAGE", "RADIAL_DISTANCE_IMAGE", "XYZ", "EXTRINSIC_CALIB", } | List of buffer_id strings denoting the wanted buffers. | -| ~/diag_mode | string: "async" or "periodic" | "async" | Diagnostic mode: asynchronous monitoring ("async") or periodic polling ("periodic"). | -| ~/xmlrpc_port| unint | 50010 | TCP port the on-camera xmlrpc server is listening on | - -## Details on the published transforms - -The ifm3d-ros2 node published three transforms: the `cloud_link`, the `mounting_link` and the `optical_link`. To clarify which frame each of these transform refers to, consider the drawings below: - -![Description of the cloud, mounting and optical frames](figures/transforms-1.png) - - -The reference of the mounting frame is at the back of the O3R camera head housing (scale drawings can be found on ifm.com in the download section of the specific article). The reference for the cloud frame is defined by the extrinsic parameters set in the JSON configuration of the O3R platform (`extrinsicHeadToUser`). Generally, the cloud is configured to have for origin the center of the robot coordinate system. When no extrinsic parameter is set, the origin of the point cloud is the back of the camera head. In this case, the cloud link and the mounting link are the same. - -![Focused description of the optical and mounting frames](figures/transforms-2.png) - -The optical frame refers to the reference point of the optical system (lens, chip, etc). A static transform is published between the mounting link and the optical link, that corresponds to the intrinsic calibration parameters of the camera. Each set of intrinsic parameters is unique to a specific camera head and set in production. These parameters are not expected to change over time. -Note that the intrinsic parameters are split into `ExtrinsicOpticToUser` (rotations and translations of the optical system in the housing) and `Intrinsics` (specific parameters of the lens, chip, etc). For more details on the calibrations, refer to the [calibration documentation](https://ifm3d.com/latest/CalibrationRoutines/index_calibrations.html). diff --git a/doc/rpc_error_codes.md b/doc/rpc_error_codes.md deleted file mode 100644 index 2431315..0000000 --- a/doc/rpc_error_codes.md +++ /dev/null @@ -1,5 +0,0 @@ -# Diagnostic - -The ifm3d-ros2 package periodically publishes diagnostic information. The diagnostic message contains an error code and a message, referring directly to an error from ifm3d or from the embedded software. -For more details on the error codes and potential troubleshooting strategies, refer to the [O3R diagnostic documentation](../../../documentation/SoftwareInterfaces/ifmDiagnostic/index_diagnostic.md). - diff --git a/doc/services.md b/doc/services.md index 9cfb955..c873895 100644 --- a/doc/services.md +++ b/doc/services.md @@ -1,28 +1,25 @@ # Advertised Services + | Name | Service Definition | Description | | ------- | ------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------- | -| Dump | [ifm3d/Dump](../srv/Dump.srv) | Dumps the state of the camera parameters to JSON | -| Config | [ifm3d/Config](../srv/Config.srv) | Provides a means to configure the camera and imager settings, declaratively from a JSON encoding of the desired settings. | -| Softon | [ifm3d/Softon](../srv/Softon.srv) | Provides a means to quickly change the camera state from IDLE to RUN. | -| Softoff | [ifm3d/Softoff](../srv/Softoff.srv) | Provides a means to quickly change the camera state from RUN to IDLE. | +| `Dump` | ifm3d_ros2/srv/Dump | Dumps the state of the camera parameters to JSON | +| `Config` | ifm3d_ros2/srv/Config | Provides a means to configure the camera and imager settings, declaratively from a JSON encoding of the desired settings. | +| `GetDiag` | ifm3d_ros2/srv/GetDiag | Get all the diagnostic messages corresponding to a JSON filter.
The filter can be left blank to get all active and dormant error messages: `ros2 service call /ifm3d/camera/GetDiag ifm3d_ros2/srv/GetDiag "{filter: '{}'}"`.| +| `Softon` | ifm3d_ros2/srv/Softon | Provides a means to quickly change the camera state from IDLE to RUN. | +| `Softoff` | ifm3d_ros2/srv/Softoff | Provides a means to quickly change the camera state from RUN to IDLE. | ## Dump and Config -The ifm3d_ros2 package allows the user to configure the O3R camera platform via two separate ways: -1. Use ROS native service calls -2. Use the dump and config service proxies - -### ROS native service calls +The ifm3d_ros2 package allows the user to configure the O3R camera platform using ROS native service calls. -#### Dump +### Dump Calling the native ROS service `/ifm3d_ros2/camera/Dump` for a certain `camera` will return the camera configuration as a JSON string. Please notice the use of backslashes (`\` before each `"`) to escape each upper quotation mark. This is necessary to allow us to keep the JSON syntax native to the underlying API (ifm3d). Call this service via, e.g. for camera: ```bash $ ros2 service call /ifm3d/camera/Dump ifm3d_ros2/srv/Dump -1637355550.468025 [0] ros2: using network interface enp0s31f6 (udp/192.168.0.10) selected arbitrarily from: enp0s31f6, wlp0s20f3, docker0, enx98fc84eebfc8 requester: making request: ifm3d_ros2.srv.Dump_Request() response: @@ -30,218 +27,27 @@ ifm3d_ros2.srv.Dump_Response(status=0, config='{"device":{"clock":{"currentTime" ``` -#### Config +### Config Below you can see an example on how to configure your camera via a ROS service call. The JSON string can be a partial JSON string. It only needs to follow basic JSON syntax. Please wrap the JSON string in a YAML syntax and use the field `"json"`. ```bash $ ros2 service call /ifm3d/camera/Config ifm3d_ros2/srv/Config "{json: '{\"ports\":{\"port0\":{\"mode\":\"standard_range4m\"}}}'}" -1637355711.266033 [0] ros2: using network interface enp0s31f6 (udp/192.168.0.10) selected arbitrarily from: enp0s31f6, wlp0s20f3, docker0, enx98fc84eebfc8 requester: making request: ifm3d_ros2.srv.Config_Request(json='{"ports":{"port2":{"mode":"standard_range4m"}}}') response: ifm3d_ros2.srv.Config_Response(status=0, msg='OK') ``` +## Diagnostic +The `ifm3d_ros2` package provides a service to poll diagnostic data from the device. A filter can be provided to retrieve specific diagnostic data. -### Dump and config service proxies -`ifm3d-ros2` provides access to each camera parameter via the `Dump` and `Config` services exposed by the `camera_node`. - -Additionally, command-line scripts called `dump` and `config` are provided as wrapper interfaces to the native API `ifm3d`. This gives a feel similar to using the underlying C++ API's command-line tool, from the ROS-independent driver except proxying the calls through the ROS network. - -For example, to dump the state of the camera: -(exemplary output from an O3R perception platform with one camera head connected is shown) - -```bash -$ ros2 run ifm3d_ros2 dump -{ - "device": { - "clock": { - "currentTime": 1581193835185485800 - }, - "diagnostic": { - "temperatures": [], - "upTime": 103190000000000 - }, - "info": { - "device": "0301", - "deviceTreeBinaryBlob": "tegra186-quill-p3310-1000-c03-00-base.dtb", - "features": {}, - "name": "", - " partNumber": "M03975", - "productionState": "AA", - "serialNumber": "000201234160", - "vendor": "0001" - }, - "network": { - "authorized_keys": "", - "ipAddressConfig": 0, - "macEth0": "00:04:4B:EA:9F:0E", - "macEth1": "00:02:01:23:41:60", - "networkSpeed": 1000, - "staticIPv4Address": "192.168.0.69", - "staticIPv4Gateway": "192.168.0.201", - "staticIPv4SubNetMask": "255.255.255.0", - "useDHCP": false - }, - "state": { - "errorMessage": "", - "errorNumber": "" - }, - "swVersion": { - "kernel": "4.9.140-l4t-r32.4+gc35f5eb9d1d9", - "l4t": "r32.4.3", - "os": "0.13.13-221", - "schema": "v0.1.0", - "swu": "0.15.12" - } - }, - "ports": { - "port0": { - "acquisition": { - "framerate": 10, - "version": { - "major": 0, - "minor": 0, - "patch": 0 - } - }, - "data": { - "algoDebugConfig": {}, - "availablePCICOutput": [], - "pcicTCPPort": 50010 - }, - "info": { - "device": "2301", - "deviceTreeBinaryBlobOverlay": "001-ov9782.dtbo", - "features": { - "fov": { - "horizontal": 127, - "vertical": 80 - }, - " resolution": { - "height": 800, - "width": 1280 - }, - "type": "2D" - }, - "name": "", - "partNumber": "M03969", - "productionState": "AA", - "sensor": "OV9782", - "sensorID": "OV9782_127x80_noIllu_Csample", - "serialNumber": "000000000382", - "vendor": "0001" - }, - "mode": "experimental_autoexposure2D", - "processing": { - "extrinsicHeadToUser": { - "rotX": 0, - "rotY": 0, - "rotZ": 0, - " transX": 0, - "transY": 0, - "transZ": 0 - }, - "version": { - "major": 0, - "minor": 0, - " patch": 0 - } - }, - "state": "RUN" - }, - "port2": { - "acquisition": { - "exposureLong": 5000, - " exposureShort": 400, - "framerate": 10, - "offset": 0, - "version": { - "major": 0, - " minor": 0, - "patch": 0 - } - }, - "data": { - "algoDebugConfig": {}, - "availablePCICOutput": [], - "pcicTCPPort": 50012 - }, - "info": { - "device": "3101", - "deviceTreeBinaryBlobOverlay": "001-irs2381c.dtbo", - "features": { - "fov": { - "horizontal": 105, - "vertical": 78 - }, - " resolution": { - "height": 172, - "width": 224 - }, - "type": "3D" - }, - "name": "", - "partNumber": "M03969", - "productionState": "AA", - "sensor": "IRS2381C", - "sensorID": "IRS2381C_105x78_4x2W_110x90_C7", - "serialNumber": "000000000382", - "vendor": "0001" - }, - "mode": "standard_range4m", - "processing": { - "diParam": { - "anfFilterSizeDiv2": 2, - "enableDynamicSymmetry": true, - "enableStraylight": true, - "enableTemporalFilter": true, - "excessiveCorrectionThreshAmp": 0.3, - "excessiveCorrectionThreshDist": 0.08, - "maxDistNoise": 0.02, - "maxSymmetry": 0.4, - "medianSizeDiv2": 0, - "minAmplitude": 20, - "minReflectivity": 0, - "mixedPixelFilterMode": 1, - "mixedPixelThresholdRad": 0.15 - }, - "extrinsicHeadToUser": { - "rotX": 1, - "rotY": 0, - "rotZ": 0, - "transX": 100, - "transY": 0, - "transZ": 0 - }, - "version": { - " major": 0, - "minor": 0, - "patch": 0 - } - }, - "state": "RUN" - } - } -} +For example, to retrieve all active diagnostic corresponding to port 0: ``` +$ ros2 service call /ifm3d/camera/GetDiag ifm3d_ros2/srv/GetDiag "{filter: '{\"source\":\"/ports/port0\", \"state\":\"active\"}'}" -Chaining together Linux pipelines works just as it does in `ifm3d`. For example, using a combination of `dump` and `config` to change the framerate -from 5Hz to 10Hz of the single application on this particular camera would look like: +requester: making request: ifm3d_ros2.srv.GetDiag_Request(filter='{"source":"/ports/port0", "state":"active"}') -```bash -$ ros2 run ifm3d_ros2 dump | jq '.ports.port0.mode="standard_range2m"' | ros2 run ifm3d_ros2 config -$ ros2 run ifm3d_ros2 dump | jq .ports.port0.mode -"standard_range2m" -``` - -:::{note} -If you do not have `jq` on your system, it can be installed with: `$ sudo apt install jq`. -::: - -For the `config` command, one difference between our ROS implementation and the `ifm3d` implementation is that we only accept input on `stdin`. So, if you had a file with JSON you wish to configure your camera with, you would simply use the file I/O redirection facilities of your shell (or something like `cat`) to feed the data to `stdin`. For example, in `bash`: - -```bash -$ ros2 run ifm3d_ros2 config < camera.json +response: +ifm3d_ros2.srv.GetDiag_Response(status=0, msg='{"bootid":"3202350a-a620-40a0-9114-5221bb55b1ff","events":[],"timestamp":1651187568817246656,"version": {"diagnostics":"0.0.11","euphrates":"1.34.32","firmware":"1.4.30.4443"}}') ``` \ No newline at end of file diff --git a/doc/topics.md b/doc/topics.md deleted file mode 100644 index 6658c76..0000000 --- a/doc/topics.md +++ /dev/null @@ -1,27 +0,0 @@ -# Topics - -## Published Topics - -| Name | Data Type | Description | -| --------------- | ---------------------------- | ------------------------------------------------------------------- | -| amplitude | sensor_msgs/msg/Image | The normalized amplitude image | -| cloud | sensor_msgs/msg/PointCloud2 | The point cloud data | -| confidence | sensor_msgs/msg/Image | The confidence image | -| distance | sensor_msgs/msg/Image | The radial distance image | -| raw_amplitude | sensor_msgs/msg/Image | The raw amplitude image (currently not available for the O3R) | -| rgb | sensor_msgs/msg/Image | The RGB 2D image of the 2D imager | -| extrinsics | ifm3d_ros2::msg::Extrinsics | The extrinsic calibration of the camera (camera to world) | -| INTRINSIC_CALIB | ifm3d_ros2::msg::Intrinsics | The intrinsic calibration of the camera (optical system parameters) | -| INVERSE_INTRINSIC_CALIBRATION | ifm3d_ros2::msg::InverseIntrinsics | The inverse intrinsic calibration of the camera | -| camera_info | sensor_msgs::msg::CameraInfo | The camera info topic containing the distortion model. This topic is published if the INTRINSIC_CALIB is part of the buffer list | -| TOF_INFO | ifm3d_ros2::msg::TOFInfo | A topic gathering various information from the tof camera (see [TOFinfo.msg](../msg/TOFInfo.msg)) | -| RGB_INFO | ifm3d_ros2::msg::RGBInfo | A topic gathering various information from the rgb camera (see [RGBInfo.msg](../msg/RGBInfo.msg)) | -| diagnostics | diagnostic_msgs::msg::DiagnosticArray | Diagnostic messages pulled from the device every second | - -:::{note} -All the topics are published with QoS `ifm3d_ros2::LowLatencyQoS`. -::: - -## Subscribed Topics - -None. diff --git a/doc/visualization.md b/doc/visualization.md index 6caf292..28d46bb 100644 --- a/doc/visualization.md +++ b/doc/visualization.md @@ -1,4 +1,5 @@ # How to visualize data with RVIZ + The included launch file `camera.launch.py` will publish and remap all topics and services to `/ifm3d/camera`, for example the point cloud topic will be remapped to `/ifm3d/camera/cloud`. Both the namespace (default: `ifm3d`) and the node name (default: `camera`) can be changed via the launch description files. @@ -7,6 +8,7 @@ You have to change the topic subscriptions yourself when using non-default value ```bash ros2 launch ifm3d_ros2 camera.launch.py visualization:=true ``` + ## Viewing the RGB image The RGB image is published on the `/ifm3d/camera*/rgb` topic in a compressed JPEG format. @@ -19,10 +21,3 @@ $ ros2 run image_transport republish compressed raw --ros-args --remap /in/compr ``` In RViz, you can now subscribe to the `/uncompressed_rgb` topic to visualize the RGB image. -## QoS reliability - best effort -When you open RVIZ for the first time and subscribe to the point cloud topic, it will not be displayed as we need to change the default quality of service (QoS) settings per topic. - -To change these settings: -1. subscribe to a topic in RVIZ by adding it (ADD button) -2. Expand the topic settings -3. Select `reliability` and set it to `Best Effort` diff --git a/examples/compute_cartesian.py b/examples/compute_cartesian.py deleted file mode 100644 index 8a6a19b..0000000 --- a/examples/compute_cartesian.py +++ /dev/null @@ -1,133 +0,0 @@ -#!/usr/bin/env python3 - -# -# Example showing how to compute the Cartesian data (point cloud) off-board the -# camera from the Unit Vectors, Extrinsics Calibration, and Distance Image. -# -import sys -import threading -import numpy as np - -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy -from sensor_msgs.msg import Image -from ifm3d_ros2.msg import Extrinsics -from cv_bridge import CvBridge - -# -# NOTE: As of the date of writing this script, the ROS2 python implementation -# of message_filters.TimeSchronizer had issues (i.e., did not handle QoS -# properly and a few other things). So, I am manually time syncing the -# extrinsics and the radial distance data. -# - -class CartesianCompute(object): - - def __init__(self, node): - self.node_ = node - - self.topic_uvec_ = "/ifm3d/camera/unit_vectors" - self.topic_extr_ = "/ifm3d/camera/extrinsics" - self.topic_dist_ = "/ifm3d/camera/distance" - - self.bridge_ = CvBridge() - - self.lock_ = threading.Lock() - self.uvec_ = None - self.extr_ = None - - self.sub_uvec_ = self.node_.create_subscription( - Image, - self.topic_uvec_, - self.uvec_cb, - QoSProfile( - depth=1, - reliability=QoSReliabilityPolicy.RELIABLE, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL - ) - ) - - qos = QoSProfile( - depth=2, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - durability=QoSDurabilityPolicy.VOLATILE - ) - - self.sub_extr_ = self.node_.create_subscription( - Extrinsics, self.topic_extr_, self.extr_cb, qos) - - self.sub_dist_ = self.node_.create_subscription( - Image, self.topic_dist_, self.dist_cb, qos) - - def uvec_cb(self, msg): - with self.lock_: - self.uvec_ = self.bridge_.imgmsg_to_cv2(msg) - - def extr_cb(self, msg): - with self.lock_: - self.extr_ = msg - - def dist_cb(self, msg): - dist = None - tx = None - ty = None - tz = None - - with self.lock_: - if ((self.extr_ is not None) and (self.uvec_ is not None)): - if msg.header.stamp == self.extr_.header.stamp: - dist = self.bridge_.imgmsg_to_cv2(msg) - tx = self.extr_.tx - ty = self.extr_.ty - tz = self.extr_.tz - else: - return - else: - return - - # unit vectors - ex = self.uvec_[:,:,0] - ey = self.uvec_[:,:,1] - ez = self.uvec_[:,:,2] - - # cast distance image to float - rdis_f = dist.astype(np.float32) - if (dist.dtype == np.float32): - # assume dist was in meters, convert to mm - rdis_f *= 1000. - - # compute cartesian - x_ = ex * rdis_f + tx - y_ = ey * rdis_f + ty - z_ = ez * rdis_f + tz - - # convert to camera frame - x = z_ - y = -x_ - z = -y_ - - # print results in meters - print(np.dstack((x,y,z))/1000.) - -def main(): - rclpy.init() - node = Node('cartesian_compute') - - try: - c = CartesianCompute(node) - rclpy.spin(node) - - except KeyboardInterrupt: - pass - - finally: - node.destroy_node() - rclpy.shutdown() - - return 0 - -if __name__ == '__main__': - sys.exit(main()) diff --git a/examples/latched_subscriber.py b/examples/latched_subscriber.py deleted file mode 100644 index ff37ba7..0000000 --- a/examples/latched_subscriber.py +++ /dev/null @@ -1,47 +0,0 @@ -#!/usr/bin/env python3 - -# -# Quick script to test that latching the unit vectors is working. Simply run -# the script once the camera_node is up. This will act as a late -# subscriber. You should see the unit vectors Image message serialized to the -# screen. Press Ctl-C to exit. -# - -import sys - -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy -from sensor_msgs.msg import Image - -def main(args=None): - topic = '/ifm3d/camera/unit_vectors' - topic_type = Image - - rclpy.init(args=args) - qos_profile = QoSProfile( - depth=1, - reliability=QoSReliabilityPolicy.RELIABLE, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL - ) - node = Node('uvec_listener') - sub = node.create_subscription( - topic_type, - topic, - lambda msg: print(msg), - qos_profile) - - try: - rclpy.spin(node) - - except KeyboardInterrupt: - node.destroy_node() - rclpy.shutdown() - - finally: - return 0 - -if __name__ == "__main__": - sys.exit(main()) diff --git a/include/ifm3d_ros2/buffer_conversions.hpp b/include/ifm3d_ros2/buffer_conversions.hpp index fc75bda..c78e7d4 100644 --- a/include/ifm3d_ros2/buffer_conversions.hpp +++ b/include/ifm3d_ros2/buffer_conversions.hpp @@ -1,446 +1,125 @@ // -*- c++ -*- /* * SPDX-License-Identifier: Apache-2.0 - * Copyright (C) 2019 ifm electronic, gmbh + * Copyright (C) 2024 ifm electronic, gmbh */ #ifndef IFM3D_ROS2_BUFFER_CONVERSIONS_HPP_ #define IFM3D_ROS2_BUFFER_CONVERSIONS_HPP_ #include -#include +#include +#include +#include +#include + #include +#include +#include +#include +#include +#include + #include -#include -#include #include #include #include #include +#include +#include +#include +#include + +using TimePointT = std::chrono::time_point; namespace ifm3d_ros2 { -sensor_msgs::msg::Image ifm3d_to_ros_image(ifm3d::Buffer& image, // Need non-const image because image.begin(), - // image.end() don't have const overloads. - const std_msgs::msg::Header& header, const rclcpp::Logger& logger) -{ - static constexpr auto max_pixel_format = static_cast(ifm3d::pixel_format::FORMAT_32F3); - static constexpr auto image_format_info = [] { - auto image_format_info = std::array{}; - - { - using namespace ifm3d; - using namespace sensor_msgs::image_encodings; - image_format_info[static_cast(pixel_format::FORMAT_8U)] = TYPE_8UC1; - image_format_info[static_cast(pixel_format::FORMAT_8S)] = TYPE_8SC1; - image_format_info[static_cast(pixel_format::FORMAT_16U)] = TYPE_16UC1; - image_format_info[static_cast(pixel_format::FORMAT_16S)] = TYPE_16SC1; - image_format_info[static_cast(pixel_format::FORMAT_32U)] = "32UC1"; - image_format_info[static_cast(pixel_format::FORMAT_32S)] = TYPE_32SC1; - image_format_info[static_cast(pixel_format::FORMAT_32F)] = TYPE_32FC1; - image_format_info[static_cast(pixel_format::FORMAT_64U)] = "64UC1"; - image_format_info[static_cast(pixel_format::FORMAT_64F)] = TYPE_64FC1; - image_format_info[static_cast(pixel_format::FORMAT_16U2)] = TYPE_16UC2; - image_format_info[static_cast(pixel_format::FORMAT_32F3)] = TYPE_32FC3; - } - - return image_format_info; - }(); - - const auto format = static_cast(image.dataFormat()); - - sensor_msgs::msg::Image result{}; - result.header = header; - result.height = image.height(); - result.width = image.width(); - result.is_bigendian = 0; - - if (image.begin() == image.end()) - { - return result; - } - - if (format >= max_pixel_format) - { - RCLCPP_ERROR(logger, "Pixel format out of range (%ld >= %ld)", format, max_pixel_format); - return result; - } - - result.encoding = image_format_info.at(format); - result.step = result.width * sensor_msgs::image_encodings::bitDepth(image_format_info.at(format)) / 8; - result.data.insert(result.data.end(), image.ptr<>(0), std::next(image.ptr<>(0), result.step * result.height)); - - if (result.encoding.empty()) - { - RCLCPP_WARN(logger, "Can't handle encoding %ld (32U == %ld, 64U == %ld)", format, - static_cast(ifm3d::pixel_format::FORMAT_32U), - static_cast(ifm3d::pixel_format::FORMAT_64U)); - result.encoding = sensor_msgs::image_encodings::TYPE_8UC1; - } - - return result; -} - +sensor_msgs::msg::Image ifm3d_to_ros_image(ifm3d::Buffer& image, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger); sensor_msgs::msg::Image ifm3d_to_ros_image(ifm3d::Buffer&& image, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) -{ - return ifm3d_to_ros_image(image, header, logger); -} - -sensor_msgs::msg::CompressedImage ifm3d_to_ros_compressed_image(ifm3d::Buffer& image, // Need non-const image because - // image.begin(), image.end() - // don't have const overloads. + const rclcpp::Logger& logger); +sensor_msgs::msg::CompressedImage ifm3d_to_ros_compressed_image(ifm3d::Buffer& image, const std_msgs::msg::Header& header, - const std::string& format, // "jpeg" or "png" - const rclcpp::Logger& logger) -{ - sensor_msgs::msg::CompressedImage result{}; - result.header = header; - result.format = format; - - if (const auto dataFormat = image.dataFormat(); - dataFormat != ifm3d::pixel_format::FORMAT_8S && dataFormat != ifm3d::pixel_format::FORMAT_8U) - { - RCLCPP_ERROR(logger, "Invalid data format for %s data (%ld)", format.c_str(), static_cast(dataFormat)); - return result; - } - - result.data.insert(result.data.end(), image.ptr<>(0), std::next(image.ptr<>(0), image.width() * image.height())); - return result; -} - + const std::string& format, + const rclcpp::Logger& logger); sensor_msgs::msg::CompressedImage ifm3d_to_ros_compressed_image(ifm3d::Buffer&& image, const std_msgs::msg::Header& header, - const std::string& format, const rclcpp::Logger& logger) -{ - return ifm3d_to_ros_compressed_image(image, header, format, logger); -} - -sensor_msgs::msg::PointCloud2 ifm3d_to_ros_cloud(ifm3d::Buffer& image, // Need non-const image because image.begin(), - // image.end() don't have const overloads. - const std_msgs::msg::Header& header, const rclcpp::Logger& logger) -{ - sensor_msgs::msg::PointCloud2 result{}; - result.header = header; - result.height = image.height(); - result.width = image.width(); - result.is_bigendian = false; - - if (image.begin() == image.end()) - { - return result; - } - - if (image.dataFormat() != ifm3d::pixel_format::FORMAT_32F3 && image.dataFormat() != ifm3d::pixel_format::FORMAT_32F) - { - RCLCPP_ERROR(logger, "Unsupported pixel format %ld for point cloud", static_cast(image.dataFormat())); - return result; - } - - sensor_msgs::msg::PointField x_field{}; - x_field.name = "x"; - x_field.offset = 0; - x_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - x_field.count = 1; - - sensor_msgs::msg::PointField y_field{}; - y_field.name = "y"; - y_field.offset = 4; - y_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - y_field.count = 1; - - sensor_msgs::msg::PointField z_field{}; - z_field.name = "z"; - z_field.offset = 8; - z_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - z_field.count = 1; - - result.fields = { - x_field, - y_field, - z_field, - }; - - result.point_step = result.fields.size() * sizeof(float); - result.row_step = result.point_step * result.width; - result.is_dense = true; - result.data.insert(result.data.end(), image.ptr<>(0), std::next(image.ptr<>(0), result.row_step * result.height)); - - return result; -} - + const std::string& format, + const rclcpp::Logger& logger); +sensor_msgs::msg::PointCloud2 ifm3d_to_ros_cloud(ifm3d::Buffer& image, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger); sensor_msgs::msg::PointCloud2 ifm3d_to_ros_cloud(ifm3d::Buffer&& image, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) -{ - return ifm3d_to_ros_cloud(image, header, logger); -} - + const rclcpp::Logger& logger); +nav_msgs::msg::MapMetaData ifm3d_to_ros_map_meta_data(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger); +nav_msgs::msg::MapMetaData ifm3d_to_ros_map_meta_data(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger); +nav_msgs::msg::OccupancyGrid ifm3d_to_ros_occupancy_grid(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger); +nav_msgs::msg::OccupancyGrid ifm3d_to_ros_occupancy_grid(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger); ifm3d_ros2::msg::Extrinsics ifm3d_to_extrinsics(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) -{ - ifm3d_ros2::msg::Extrinsics extrinsics_msg; - extrinsics_msg.header = header; - try - { - extrinsics_msg.tx = buffer.at(0); - extrinsics_msg.ty = buffer.at(1); - extrinsics_msg.tz = buffer.at(2); - extrinsics_msg.rot_x = buffer.at(3); - extrinsics_msg.rot_y = buffer.at(4); - extrinsics_msg.rot_z = buffer.at(5); - } - catch (const std::out_of_range& ex) - { - RCLCPP_WARN(logger, "Out-of-range error fetching extrinsics"); - } - return extrinsics_msg; -} - + const rclcpp::Logger& logger); ifm3d_ros2::msg::Extrinsics ifm3d_to_extrinsics(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) -{ - return ifm3d_to_extrinsics(buffer, header, logger); -} - -sensor_msgs::msg::CameraInfo ifm3d_to_camera_info(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, - const uint32_t height, const uint32_t width, - const rclcpp::Logger& logger) -{ - ifm3d::calibration::IntrinsicCalibration intrinsic; - intrinsic.Read(buffer.ptr(0)); - - sensor_msgs::msg::CameraInfo camera_info_msg; - camera_info_msg.header = header; - - try - { - camera_info_msg.height = height; - camera_info_msg.width = width; - camera_info_msg.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; - - // Read data from buffer - const float fx = intrinsic.model_parameters[0]; - const float fy = intrinsic.model_parameters[1]; - const float mx = intrinsic.model_parameters[2]; - const float my = intrinsic.model_parameters[3]; - const float alpha = intrinsic.model_parameters[4]; - const float k1 = intrinsic.model_parameters[5]; - const float k2 = intrinsic.model_parameters[6]; - const float k3 = intrinsic.model_parameters[7]; - const float k4 = intrinsic.model_parameters[8]; - // next in buffer is k5 for bouguet or theta_max for fisheye model, both not needed here - - const float ix = width - 1; - const float iy = height - 1; - const float cy = (iy + 0.5 - my) / fy; - const float cx = (ix + 0.5 - mx) / fx - alpha * cy; - const float r2 = cx * cx + cy * cy; - const float h = 2 * cx * cy; - const float tx = k3 * h + k4 * (r2 + 2 * cx * cx); - const float ty = k3 * (r2 + 2 * cy * cy) + k4 * h; - - // Distortion parameters - camera_info_msg.d.resize(5); - camera_info_msg.d[0] = k1; - camera_info_msg.d[1] = k2; - camera_info_msg.d[2] = tx; // TODO t1 == tx ? - camera_info_msg.d[3] = ty; // TODO t2 == ty ? - camera_info_msg.d[4] = k3; - - // Intrinsic camera matrix - camera_info_msg.k[0] = fx; - camera_info_msg.k[4] = fy; - camera_info_msg.k[2] = cx; - camera_info_msg.k[5] = cy; - camera_info_msg.k[8] = 1.0; // fixed to 1.0 - - // Projection matrix - camera_info_msg.p[0] = fx; - camera_info_msg.p[5] = fy; - camera_info_msg.p[2] = cx; - camera_info_msg.p[6] = cy; - camera_info_msg.p[10] = 1.0; // fixed to 1.0 - - RCLCPP_DEBUG_ONCE(logger, - "Intrinsics:\nfx=%f \nfy=%f \nmx=%f \nmy=%f \nalpha=%f \nk1=%f \nk2=%f \nk3=%f \nk4=%f " - "\nCalculated:\nix=%f \niy=%f \ncx=%f \ncy=%f \nr2=%f \nh=%f \ntx=%f \nty=%f", - fx, fy, mx, my, alpha, k1, k2, k3, k4, ix, iy, cx, cy, r2, h, tx, ty); - } - catch (const std::out_of_range& ex) - { - RCLCPP_WARN(logger, "Out-of-range error fetching intrinsics"); - } - - return camera_info_msg; -} - -sensor_msgs::msg::CameraInfo ifm3d_to_camera_info(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, - const uint32_t height, const uint32_t width, - const rclcpp::Logger& logger) - -{ - return ifm3d_to_camera_info(buffer, header, height, width, logger); -} - + const rclcpp::Logger& logger); +bool ifm3d_rgb_info_to_camera_info(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, const uint32_t height, + const uint32_t width, const rclcpp::Logger& logger, + sensor_msgs::msg::CameraInfo& camera_info_msg); +bool ifm3d_rgb_info_to_camera_info(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, const uint32_t height, + const uint32_t width, const rclcpp::Logger& logger, + sensor_msgs::msg::CameraInfo& camera_info_msg); +bool ifm3d_tof_info_to_camera_info(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, const uint32_t height, + const uint32_t width, const rclcpp::Logger& logger, + sensor_msgs::msg::CameraInfo& camera_info_msg); +bool ifm3d_tof_info_to_camera_info(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, const uint32_t height, + const uint32_t width, const rclcpp::Logger& logger, + sensor_msgs::msg::CameraInfo& camera_info_msg); +sensor_msgs::msg::CameraInfo ifm3d_to_camera_info(ifm3d::calibration::IntrinsicCalibration intrinsic, + const std_msgs::msg::Header& header, const uint32_t height, + const uint32_t width, const rclcpp::Logger& logger); ifm3d_ros2::msg::Intrinsics ifm3d_to_intrinsics(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) -{ - ifm3d_ros2::msg::Intrinsics intrinsics_msg; - intrinsics_msg.header = header; - - ifm3d::calibration::IntrinsicCalibration intrinsics; - - try - { - intrinsics.Read(buffer.ptr(0)); - intrinsics_msg.model_id = intrinsics.model_id; - intrinsics_msg.model_parameters = intrinsics.model_parameters; - } - catch (...) - { - RCLCPP_ERROR(logger, "Failed to read intrinsics."); - } - - return intrinsics_msg; -} - + const rclcpp::Logger& logger); ifm3d_ros2::msg::Intrinsics ifm3d_to_intrinsics(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) - -{ - return ifm3d_to_intrinsics(buffer, header, logger); -} - + const rclcpp::Logger& logger); ifm3d_ros2::msg::InverseIntrinsics ifm3d_to_inverse_intrinsics(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) -{ - ifm3d_ros2::msg::InverseIntrinsics inverse_intrinsics_msg; - inverse_intrinsics_msg.header = header; - - ifm3d::calibration::InverseIntrinsicCalibration inverse_intrinsics; - - try - { - inverse_intrinsics.Read(buffer.ptr(0)); - inverse_intrinsics_msg.model_id = inverse_intrinsics.model_id; - inverse_intrinsics_msg.model_parameters = inverse_intrinsics.model_parameters; - } - catch (...) - { - RCLCPP_ERROR(logger, "Failed to read inverse intrinsics."); - } - - return inverse_intrinsics_msg; -} - + const rclcpp::Logger& logger); ifm3d_ros2::msg::InverseIntrinsics ifm3d_to_inverse_intrinsics(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) - -{ - return ifm3d_to_inverse_intrinsics(buffer, header, logger); -} - + const rclcpp::Logger& logger); ifm3d_ros2::msg::RGBInfo ifm3d_to_rgb_info(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) -{ - ifm3d_ros2::msg::RGBInfo rgb_info_msg; - rgb_info_msg.header = header; - - try - { - auto rgb_info = ifm3d::RGBInfoV1::Deserialize(buffer); - - rgb_info_msg.version = rgb_info.version; - rgb_info_msg.frame_counter = rgb_info.frame_counter; - rgb_info_msg.timestamp_ns = rgb_info.timestamp_ns; - rgb_info_msg.exposure_time = rgb_info.exposure_time; - - rgb_info_msg.extrinsics.header = header; - rgb_info_msg.extrinsics.tx = rgb_info.extrinsic_optic_to_user.trans_x; - rgb_info_msg.extrinsics.ty = rgb_info.extrinsic_optic_to_user.trans_y; - rgb_info_msg.extrinsics.tz = rgb_info.extrinsic_optic_to_user.trans_z; - rgb_info_msg.extrinsics.rot_x = rgb_info.extrinsic_optic_to_user.rot_x; - rgb_info_msg.extrinsics.rot_y = rgb_info.extrinsic_optic_to_user.rot_y; - rgb_info_msg.extrinsics.rot_z = rgb_info.extrinsic_optic_to_user.rot_z; - - rgb_info_msg.intrinsics.header = header; - rgb_info_msg.intrinsics.model_id = rgb_info.intrinsic_calibration.model_id; - rgb_info_msg.intrinsics.model_parameters = rgb_info.intrinsic_calibration.model_parameters; - - rgb_info_msg.inverse_intrinsics.header = header; - rgb_info_msg.inverse_intrinsics.model_id = rgb_info.inverse_intrinsic_calibration.model_id; - rgb_info_msg.inverse_intrinsics.model_parameters = rgb_info.inverse_intrinsic_calibration.model_parameters; - } - catch (...) - { - RCLCPP_ERROR(logger, "Failed to read rgb info."); - } - - return rgb_info_msg; -} - + const rclcpp::Logger& logger); ifm3d_ros2::msg::RGBInfo ifm3d_to_rgb_info(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) - -{ - return ifm3d_to_rgb_info(buffer, header, logger); -} - + const rclcpp::Logger& logger); ifm3d_ros2::msg::TOFInfo ifm3d_to_tof_info(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) -{ - ifm3d_ros2::msg::TOFInfo tof_info_msg; - tof_info_msg.header = header; - - try - { - auto tof_info = ifm3d::TOFInfoV4::Deserialize(buffer); - tof_info_msg.measurement_block_index = tof_info.measurement_block_index; - tof_info_msg.measurement_range_min = tof_info.measurement_range_min; - tof_info_msg.measurement_range_max = tof_info.measurement_range_max; - tof_info_msg.version = tof_info.version; - tof_info_msg.distance_resolution = tof_info.distance_resolution; - tof_info_msg.amplitude_resolution = tof_info.amplitude_resolution; - tof_info_msg.amp_normalization_factors = tof_info.amp_normalization_factors; - tof_info_msg.exposure_timestamps_ns = tof_info.exposure_timestamps_ns; - tof_info_msg.exposure_times_s = tof_info.exposure_times_s; - tof_info_msg.illu_temperature = tof_info.illu_temperature; - tof_info_msg.mode = std::string(std::begin(tof_info.mode), std::end(tof_info.mode)); - tof_info_msg.imager = std::string(std::begin(tof_info.imager), std::end(tof_info.imager)); - - tof_info_msg.extrinsics.header = header; - tof_info_msg.extrinsics.tx = tof_info.extrinsic_optic_to_user.trans_x; - tof_info_msg.extrinsics.ty = tof_info.extrinsic_optic_to_user.trans_y; - tof_info_msg.extrinsics.tz = tof_info.extrinsic_optic_to_user.trans_z; - tof_info_msg.extrinsics.rot_x = tof_info.extrinsic_optic_to_user.rot_x; - tof_info_msg.extrinsics.rot_y = tof_info.extrinsic_optic_to_user.rot_y; - tof_info_msg.extrinsics.rot_z = tof_info.extrinsic_optic_to_user.rot_z; - - tof_info_msg.intrinsics.header = header; - tof_info_msg.intrinsics.model_id = tof_info.intrinsic_calibration.model_id; - tof_info_msg.intrinsics.model_parameters = tof_info.intrinsic_calibration.model_parameters; - - tof_info_msg.inverse_intrinsics.header = header; - tof_info_msg.inverse_intrinsics.model_id = tof_info.inverse_intrinsic_calibration.model_id; - tof_info_msg.inverse_intrinsics.model_parameters = tof_info.inverse_intrinsic_calibration.model_parameters; - } - catch (...) - { - RCLCPP_ERROR(logger, "Failed to read tof info."); - } - - return tof_info_msg; -} - + const rclcpp::Logger& logger); ifm3d_ros2::msg::TOFInfo ifm3d_to_tof_info(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) - -{ - return ifm3d_to_tof_info(buffer, header, logger); -} - + const rclcpp::Logger& logger); +nav2_msgs::msg::Costmap ifm3d_to_ros_costmap(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger); +nav2_msgs::msg::Costmap ifm3d_to_ros_costmap(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger); +rclcpp::Time ifm3d_to_ros_time(const TimePointT& time_point); +geometry_msgs::msg::TransformStamped trans_rot_to_optical_mount_link(std::vector trans_rot, + std::uint64_t timestamp, + std::string mounting_frame_name, + std::string optical_frame_name); +bool ifm3d_extrinsic_opt_to_user_to_optical_mount_link(ifm3d::calibration::ExtrinsicOpticToUser opt_to_user, + std::uint64_t timestamp, std::string mounting_frame_name, + std::string optical_frame_name, const rclcpp::Logger& logger, + geometry_msgs::msg::TransformStamped& tf); +bool ifm3d_rgb_info_to_optical_mount_link(ifm3d::Buffer& buffer, std::string mounting_frame_name, + std::string optical_frame_name, const rclcpp::Logger& logger, + geometry_msgs::msg::TransformStamped& tf); +bool ifm3d_rgb_info_to_optical_mount_link(ifm3d::Buffer&& buffer, std::string mounting_frame_name, + std::string optical_frame_name, const rclcpp::Logger& logger, + geometry_msgs::msg::TransformStamped& tf); +bool ifm3d_tof_info_to_optical_mount_link(ifm3d::Buffer& buffer, std::string mounting_frame_name, + std::string optical_frame_name, const rclcpp::Logger& logger, + geometry_msgs::msg::TransformStamped& tf); +bool ifm3d_tof_info_to_optical_mount_link(ifm3d::Buffer&& buffer, std::string mounting_frame_name, + std::string optical_frame_name, const rclcpp::Logger& logger, + geometry_msgs::msg::TransformStamped& tf); } // namespace ifm3d_ros2 -#endif \ No newline at end of file +#endif // IFM3D_ROS2_BUFFER_CONVERSIONS_HPP_ diff --git a/include/ifm3d_ros2/buffer_id_utils.hpp b/include/ifm3d_ros2/buffer_id_utils.hpp index c39b63c..6dfae86 100644 --- a/include/ifm3d_ros2/buffer_id_utils.hpp +++ b/include/ifm3d_ros2/buffer_id_utils.hpp @@ -1,3 +1,9 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + #ifndef IFM3D_ROS2_CONSTANTS_HPP_ #define IFM3D_ROS2_CONSTANTS_HPP_ @@ -5,7 +11,9 @@ #include #include -#include "ifm3d/fg/frame.h" +#include +#include +#include namespace ifm3d_ros2 { @@ -18,6 +26,7 @@ enum data_stream_type { rgb_2d, tof_3d, + ods, }; /** @@ -25,14 +34,16 @@ enum data_stream_type */ enum message_type { - intrinsics, compressed_image, extrinsics, + intrinsics, inverse_intrinsics, + occupancy_grid, pointcloud, raw_image, rgb_info, tof_info, + zones, not_implemented, }; @@ -42,37 +53,7 @@ enum message_type * It is not declared const because the operator[] of std::map would not be available. * String are taken from the ifm3d SDK (modules/pybind11/src/bindings/frame.h) */ -std::map buffer_id_map = { - { "RADIAL_DISTANCE_IMAGE", ifm3d::buffer_id::RADIAL_DISTANCE_IMAGE }, - { "NORM_AMPLITUDE_IMAGE", ifm3d::buffer_id::NORM_AMPLITUDE_IMAGE }, - { "AMPLITUDE_IMAGE", ifm3d::buffer_id::AMPLITUDE_IMAGE }, - { "GRAYSCALE_IMAGE", ifm3d::buffer_id::GRAYSCALE_IMAGE }, - { "RADIAL_DISTANCE_NOISE", ifm3d::buffer_id::RADIAL_DISTANCE_NOISE }, - { "REFLECTIVITY", ifm3d::buffer_id::REFLECTIVITY }, - { "CARTESIAN_X_COMPONENT", ifm3d::buffer_id::CARTESIAN_X_COMPONENT }, - { "CARTESIAN_Y_COMPONENT", ifm3d::buffer_id::CARTESIAN_Y_COMPONENT }, - { "CARTESIAN_Z_COMPONENT", ifm3d::buffer_id::CARTESIAN_Z_COMPONENT }, - { "CARTESIAN_ALL", ifm3d::buffer_id::CARTESIAN_ALL }, - { "UNIT_VECTOR_ALL", ifm3d::buffer_id::UNIT_VECTOR_ALL }, - { "MONOCHROM_2D_12BIT", ifm3d::buffer_id::MONOCHROM_2D_12BIT }, - { "MONOCHROM_2D", ifm3d::buffer_id::MONOCHROM_2D }, - { "JPEG_IMAGE", ifm3d::buffer_id::JPEG_IMAGE }, - { "CONFIDENCE_IMAGE", ifm3d::buffer_id::CONFIDENCE_IMAGE }, - { "DIAGNOSTIC", ifm3d::buffer_id::DIAGNOSTIC }, - { "JSON_DIAGNOSTIC", ifm3d::buffer_id::JSON_DIAGNOSTIC }, - { "EXTRINSIC_CALIB", ifm3d::buffer_id::EXTRINSIC_CALIB }, - { "INTRINSIC_CALIB", ifm3d::buffer_id::INTRINSIC_CALIB }, - { "INVERSE_INTRINSIC_CALIBRATION", ifm3d::buffer_id::INVERSE_INTRINSIC_CALIBRATION }, - { "TOF_INFO", ifm3d::buffer_id::TOF_INFO }, - { "RGB_INFO", ifm3d::buffer_id::RGB_INFO }, - { "JSON_MODEL", ifm3d::buffer_id::JSON_MODEL }, - { "ALGO_DEBUG", ifm3d::buffer_id::ALGO_DEBUG }, - { "O3R_ODS_OCCUPANCY_GRID", ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID }, - { "O3R_ODS_INFO", ifm3d::buffer_id::O3R_ODS_INFO }, - { "XYZ", ifm3d::buffer_id::XYZ }, - { "EXPOSURE_TIME", ifm3d::buffer_id::EXPOSURE_TIME }, - { "ILLUMINATION_TEMP", ifm3d::buffer_id::ILLUMINATION_TEMP } -}; +extern std::map buffer_id_map; /** * @brief mapping buffer_ids to data_stream_type where they are available @@ -81,39 +62,7 @@ std::map buffer_id_map = { * as some buffer might be available for different data_stream_type * It is not declared const because equal_range() of std::multimap would not be available. */ -std::multimap data_stream_type_map = { - { ifm3d::buffer_id::RADIAL_DISTANCE_IMAGE, data_stream_type::tof_3d }, - { ifm3d::buffer_id::NORM_AMPLITUDE_IMAGE, data_stream_type::tof_3d }, - { ifm3d::buffer_id::AMPLITUDE_IMAGE, data_stream_type::tof_3d }, - { ifm3d::buffer_id::GRAYSCALE_IMAGE, data_stream_type::tof_3d }, - { ifm3d::buffer_id::RADIAL_DISTANCE_NOISE, data_stream_type::tof_3d }, - { ifm3d::buffer_id::REFLECTIVITY, data_stream_type::tof_3d }, - { ifm3d::buffer_id::CARTESIAN_X_COMPONENT, data_stream_type::tof_3d }, - { ifm3d::buffer_id::CARTESIAN_Y_COMPONENT, data_stream_type::tof_3d }, - { ifm3d::buffer_id::CARTESIAN_Z_COMPONENT, data_stream_type::tof_3d }, - { ifm3d::buffer_id::CARTESIAN_ALL, data_stream_type::tof_3d }, - { ifm3d::buffer_id::UNIT_VECTOR_ALL, data_stream_type::tof_3d }, - { ifm3d::buffer_id::MONOCHROM_2D_12BIT, data_stream_type::rgb_2d }, - { ifm3d::buffer_id::MONOCHROM_2D, data_stream_type::rgb_2d }, - { ifm3d::buffer_id::JPEG_IMAGE, data_stream_type::rgb_2d }, - { ifm3d::buffer_id::CONFIDENCE_IMAGE, data_stream_type::tof_3d }, - { ifm3d::buffer_id::DIAGNOSTIC, data_stream_type::tof_3d }, - { ifm3d::buffer_id::DIAGNOSTIC, data_stream_type::rgb_2d }, - { ifm3d::buffer_id::JSON_DIAGNOSTIC, data_stream_type::tof_3d }, - { ifm3d::buffer_id::JSON_DIAGNOSTIC, data_stream_type::rgb_2d }, - { ifm3d::buffer_id::EXTRINSIC_CALIB, data_stream_type::tof_3d }, - { ifm3d::buffer_id::INTRINSIC_CALIB, data_stream_type::tof_3d }, - { ifm3d::buffer_id::INVERSE_INTRINSIC_CALIBRATION, data_stream_type::tof_3d }, - { ifm3d::buffer_id::TOF_INFO, data_stream_type::tof_3d }, - { ifm3d::buffer_id::RGB_INFO, data_stream_type::rgb_2d }, - { ifm3d::buffer_id::JSON_MODEL, data_stream_type::tof_3d }, - { ifm3d::buffer_id::ALGO_DEBUG, data_stream_type::tof_3d }, - { ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID, data_stream_type::tof_3d }, - { ifm3d::buffer_id::O3R_ODS_INFO, data_stream_type::tof_3d }, - { ifm3d::buffer_id::XYZ, data_stream_type::tof_3d }, - { ifm3d::buffer_id::EXPOSURE_TIME, data_stream_type::tof_3d }, - { ifm3d::buffer_id::ILLUMINATION_TEMP, data_stream_type::tof_3d } -}; +extern std::multimap data_stream_type_map; /** * @brief mapping buffer_ids to message type enum @@ -122,37 +71,7 @@ std::multimap data_stream_type_map = { * e.g. to decide on which ROS message type shall be used. * It is not declared const because the operator[] of std::map would not be available. */ -std::map message_type_map = { - { ifm3d::buffer_id::RADIAL_DISTANCE_IMAGE, message_type::raw_image }, - { ifm3d::buffer_id::NORM_AMPLITUDE_IMAGE, message_type::raw_image }, - { ifm3d::buffer_id::AMPLITUDE_IMAGE, message_type::raw_image }, - { ifm3d::buffer_id::GRAYSCALE_IMAGE, message_type::not_implemented }, - { ifm3d::buffer_id::RADIAL_DISTANCE_NOISE, message_type::not_implemented }, - { ifm3d::buffer_id::REFLECTIVITY, message_type::not_implemented }, - { ifm3d::buffer_id::CARTESIAN_X_COMPONENT, message_type::not_implemented }, - { ifm3d::buffer_id::CARTESIAN_Y_COMPONENT, message_type::not_implemented }, - { ifm3d::buffer_id::CARTESIAN_Z_COMPONENT, message_type::not_implemented }, - { ifm3d::buffer_id::CARTESIAN_ALL, message_type::not_implemented }, - { ifm3d::buffer_id::UNIT_VECTOR_ALL, message_type::not_implemented }, - { ifm3d::buffer_id::MONOCHROM_2D_12BIT, message_type::not_implemented }, - { ifm3d::buffer_id::MONOCHROM_2D, message_type::not_implemented }, - { ifm3d::buffer_id::JPEG_IMAGE, message_type::compressed_image }, - { ifm3d::buffer_id::CONFIDENCE_IMAGE, message_type::raw_image }, - { ifm3d::buffer_id::DIAGNOSTIC, message_type::not_implemented }, - { ifm3d::buffer_id::JSON_DIAGNOSTIC, message_type::not_implemented }, - { ifm3d::buffer_id::EXTRINSIC_CALIB, message_type::extrinsics }, - { ifm3d::buffer_id::INTRINSIC_CALIB, message_type::intrinsics }, - { ifm3d::buffer_id::INVERSE_INTRINSIC_CALIBRATION, message_type::inverse_intrinsics }, - { ifm3d::buffer_id::TOF_INFO, message_type::tof_info }, - { ifm3d::buffer_id::RGB_INFO, message_type::rgb_info }, - { ifm3d::buffer_id::JSON_MODEL, message_type::not_implemented }, - { ifm3d::buffer_id::ALGO_DEBUG, message_type::not_implemented }, - { ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID, message_type::not_implemented }, - { ifm3d::buffer_id::O3R_ODS_INFO, message_type::not_implemented }, - { ifm3d::buffer_id::XYZ, message_type::pointcloud }, - { ifm3d::buffer_id::EXPOSURE_TIME, message_type::not_implemented }, - { ifm3d::buffer_id::ILLUMINATION_TEMP, message_type::not_implemented } -}; +extern std::map message_type_map; /** * @brief mapping buffer_ids to topic names @@ -161,37 +80,7 @@ std::map message_type_map = { * and backwards compatibility. * It is not declared const because the operator[] of std::map would not be available. */ -std::map topic_name_map = { - { ifm3d::buffer_id::RADIAL_DISTANCE_IMAGE, "distance" }, - { ifm3d::buffer_id::NORM_AMPLITUDE_IMAGE, "amplitude" }, - { ifm3d::buffer_id::AMPLITUDE_IMAGE, "raw_amplitude" }, - { ifm3d::buffer_id::GRAYSCALE_IMAGE, "GRAYSCALE_IMAGE" }, - { ifm3d::buffer_id::RADIAL_DISTANCE_NOISE, "RADIAL_DISTANCE_NOISE" }, - { ifm3d::buffer_id::REFLECTIVITY, "REFLECTIVITY" }, - { ifm3d::buffer_id::CARTESIAN_X_COMPONENT, "CARTESIAN_X_COMPONENT" }, - { ifm3d::buffer_id::CARTESIAN_Y_COMPONENT, "CARTESIAN_Y_COMPONENT" }, - { ifm3d::buffer_id::CARTESIAN_Z_COMPONENT, "CARTESIAN_Z_COMPONENT" }, - { ifm3d::buffer_id::CARTESIAN_ALL, "CARTESIAN_ALL" }, - { ifm3d::buffer_id::UNIT_VECTOR_ALL, "UNIT_VECTOR_ALL" }, - { ifm3d::buffer_id::MONOCHROM_2D_12BIT, "MONOCHROM_2D_12BIT" }, - { ifm3d::buffer_id::MONOCHROM_2D, "MONOCHROM_2D" }, - { ifm3d::buffer_id::JPEG_IMAGE, "rgb" }, - { ifm3d::buffer_id::CONFIDENCE_IMAGE, "confidence" }, - { ifm3d::buffer_id::DIAGNOSTIC, "DIAGNOSTIC" }, - { ifm3d::buffer_id::JSON_DIAGNOSTIC, "JSON_DIAGNOSTIC" }, - { ifm3d::buffer_id::EXTRINSIC_CALIB, "extrinsics" }, - { ifm3d::buffer_id::INTRINSIC_CALIB, "INTRINSIC_CALIB" }, - { ifm3d::buffer_id::INVERSE_INTRINSIC_CALIBRATION, "INVERSE_INTRINSIC_CALIBRATION" }, - { ifm3d::buffer_id::TOF_INFO, "TOF_INFO" }, - { ifm3d::buffer_id::RGB_INFO, "RGB_INFO" }, - { ifm3d::buffer_id::JSON_MODEL, "JSON_MODEL" }, - { ifm3d::buffer_id::ALGO_DEBUG, "ALGO_DEBUG" }, - { ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID, "O3R_ODS_OCCUPANCY_GRID" }, - { ifm3d::buffer_id::O3R_ODS_INFO, "O3R_ODS_INFO" }, - { ifm3d::buffer_id::XYZ, "cloud" }, - { ifm3d::buffer_id::EXPOSURE_TIME, "EXPOSURE_TIME" }, - { ifm3d::buffer_id::ILLUMINATION_TEMP, "ILLUMINATION_TEMP" } -}; +extern std::map topic_name_map; /** * @brief Lookup buffer_id associated with a string, using the buffer_id_map @@ -201,17 +90,7 @@ std::map topic_name_map = { * @return true if string is valid * @return false if no buffer_id is associated with the string */ -bool convert(const std::string& string, ifm3d::buffer_id& buffer_id) -{ - if (!buffer_id_map.count(string)) - { - // key does not exist - return false; - } - - buffer_id = buffer_id_map[string]; - return true; -} +bool convert(const std::string& string, ifm3d::buffer_id& buffer_id); /** * @brief Lookup string associated with a buffer_id, using the buffer_id_map @@ -221,21 +100,7 @@ bool convert(const std::string& string, ifm3d::buffer_id& buffer_id) * @return true if buffer_id is valid * @return false there is no entry in the map for the given buffer_id */ -bool convert(const ifm3d::buffer_id& buffer_id, std::string& string) -{ - // Iterate over all map entries - for (auto const& [key, value] : buffer_id_map) - { - if (value == buffer_id) - { - string = key; - return true; - } - } - - // buffer_id not found - return false; -} +bool convert(const ifm3d::buffer_id& buffer_id, std::string& string); /** * @brief Returns the subset of the provided buffer_ids, which are compatible with a given data_stream_type @@ -245,75 +110,17 @@ bool convert(const ifm3d::buffer_id& buffer_id, std::string& string) * @return std::vector subset of input_ids which is available for given type */ std::vector buffer_ids_for_data_stream_type(const std::vector& input_ids, - const data_stream_type& type) -{ - typedef std::multimap::iterator mm_iterator; - - std::vector ret_vector; - - for (ifm3d::buffer_id input_id : input_ids) - { - // Get iterators for multimap subsection of given buffer_id - // Remember, multimaps are always sorted by their key - std::pair result = data_stream_type_map.equal_range(input_id); - - // Look for matching data_streamtype, iterating over the subsection - for (mm_iterator it = result.first; it != result.second; it++) - { - if (it->second == type) - { - ret_vector.push_back(input_id); - } - } - } - - return ret_vector; -} + const data_stream_type& type); /** * Helper to create one string from a vector of strings */ -std::string vector_to_string(const std::vector& vector) -{ - if (vector.empty()) - { - return std::string(""); - } - - std::ostringstream stream; - const std::string delimiter = ", "; - - std::copy(vector.begin(), vector.end(), std::ostream_iterator(stream, delimiter.c_str())); - - const std::string& output = stream.str(); - - return output.substr(0, output.length() - delimiter.length()); -} +std::string vector_to_string(const std::vector& vector); /** * Helper to create one string from a vector of buffer_ids */ -std::string vector_to_string(const std::vector& vector) -{ - if (vector.empty()) - { - return std::string(""); - } - - std::ostringstream stream; - const std::string delimiter = ", "; - - for (const auto& id : vector) - { - std::string string; - convert(id, string); - stream << string << delimiter; - } - - const std::string& output = stream.str(); - - return output.substr(0, output.length() - delimiter.length()); -} +std::string vector_to_string(const std::vector& vector); } // namespace buffer_id_utils diff --git a/include/ifm3d_ros2/camera_node.hpp b/include/ifm3d_ros2/camera_node.hpp index 90b5d6f..04e9afd 100644 --- a/include/ifm3d_ros2/camera_node.hpp +++ b/include/ifm3d_ros2/camera_node.hpp @@ -1,7 +1,7 @@ // -*- c++ -*- /* * SPDX-License-Identifier: Apache-2.0 - * Copyright (C) 2019 ifm electronic, gmbh + * Copyright (C) 2024 ifm electronic, gmbh */ #ifndef IFM3D_ROS2_CAMERA_NODE_HPP_ @@ -14,86 +14,27 @@ #include #include -#include #include #include #include #include -#include -#include -#include -#include -#include -#include - -#include #include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include -#include -#include +#include #include #include using TC_RETVAL = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -using DiagnosticStatusMsg = diagnostic_msgs::msg::DiagnosticStatus; -using DiagnosticArrayMsg = diagnostic_msgs::msg::DiagnosticArray; -using DiagnosticArrayPublisher = std::shared_ptr>; - -using ImageMsg = sensor_msgs::msg::Image; -using ImagePublisher = std::shared_ptr>; - -using CompressedImageMsg = sensor_msgs::msg::CompressedImage; -using CompressedImagePublisher = std::shared_ptr>; - -using PCLMsg = sensor_msgs::msg::PointCloud2; -using PCLPublisher = std::shared_ptr>; - using ExtrinsicsMsg = ifm3d_ros2::msg::Extrinsics; using ExtrinsicsPublisher = std::shared_ptr>; -using CameraInfoMsg = sensor_msgs::msg::CameraInfo; -using CameraInfoPublisher = std::shared_ptr>; - -using IntrinsicsMsg = ifm3d_ros2::msg::Intrinsics; -using IntrinsicsPublisher = std::shared_ptr>; - -using InverseIntrinsicsMsg = ifm3d_ros2::msg::InverseIntrinsics; -using InverseIntrinsicsPublisher = std::shared_ptr>; - -using TOFInfoMsg = ifm3d_ros2::msg::TOFInfo; -using TOFInfoPublisher = std::shared_ptr>; - -using RGBInfoMsg = ifm3d_ros2::msg::RGBInfo; -using RGBInfoPublisher = std::shared_ptr>; - -using DumpRequest = std::shared_ptr; -using DumpResponse = std::shared_ptr; -using DumpService = ifm3d_ros2::srv::Dump; -using DumpServer = rclcpp::Service::SharedPtr; - -using ConfigRequest = std::shared_ptr; -using ConfigResponse = std::shared_ptr; -using ConfigService = ifm3d_ros2::srv::Config; -using ConfigServer = rclcpp::Service::SharedPtr; - -using SoftoffRequest = std::shared_ptr; -using SoftoffResponse = std::shared_ptr; -using SoftoffService = ifm3d_ros2::srv::Softoff; -using SoftoffServer = rclcpp::Service::SharedPtr; - -using SoftonRequest = std::shared_ptr; -using SoftonResponse = std::shared_ptr; -using SoftonService = ifm3d_ros2::srv::Softon; -using SoftonServer = rclcpp::Service::SharedPtr; - namespace ifm3d_ros2 { /** @@ -199,26 +140,6 @@ class IFM3D_ROS2_PUBLIC CameraNode : public rclcpp_lifecycle::LifecycleNode TC_RETVAL on_error(const rclcpp_lifecycle::State& prev_state) override; protected: - /** - * Implementation of the Dump service. - */ - void Dump(std::shared_ptr request_header, DumpRequest req, DumpResponse resp); - - /** - * Implementation of the Config service. - */ - void Config(std::shared_ptr request_header, ConfigRequest req, ConfigResponse resp); - - /** - * Implementation of the SoftOff service. - */ - void Softoff(std::shared_ptr request_header, SoftoffRequest req, SoftoffResponse resp); - - /** - * Implementation of the SoftOn service. - */ - void Softon(std::shared_ptr request_header, SoftonRequest req, SoftonResponse resp); - /** * Declares parameters and default values */ @@ -254,112 +175,45 @@ class IFM3D_ROS2_PUBLIC CameraNode : public rclcpp_lifecycle::LifecycleNode */ void async_notification_callback(const std::string& s1, const std::string& s2); - /** - * Creates a DiagnosticStatus message from a JSON string. - * - */ - DiagnosticStatusMsg create_diagnostic_status(const uint8_t level, const std::string& json_msg); - - /** - * @brief Create publishers according to buffer_id_list_. - * - * First, this clears internal publisher lists. - * Populates the internal publisher lists with new Publishers. - * Uses buffer_id_utils to determine message types. - */ - void initialize_publishers(); - - /** - * Activates all publishers on the internal publisher lists. - */ - void activate_publishers(); - - /** - * Deactivates all publishers on the internal publisher lists. - */ - void deactivate_publishers(); - - /** - * Publish the transform from the mounting link to the optical link as static tf - */ - void publish_optical_link_transform(); - - /** - * @brief Publish the transform from the mounting link to the cloud link as static tf if it changed - * - * A change can either be new translational/rotational data from extrinsics or - * a name change of one of the frames. - * - * @param msg ExtrinsicsMsg from camera - */ - void publish_cloud_link_transform_if_changed(const ExtrinsicsMsg& msg); - - ifm3d_ros2::buffer_id_utils::data_stream_type stream_type_from_port_info(const std::vector& ports, - const uint16_t pcic_port); + ifm3d_ros2::buffer_id_utils::data_stream_type stream_type_from_port_info(const ifm3d::PortInfo port_info); private: rclcpp::Logger logger_; - /// For diagnostics, "/" - std::string hardware_id_; - // ifm3d camera and framegrabber pointers - ifm3d::O3R::Ptr cam_{}; + ifm3d::O3R::Ptr o3r_{}; ifm3d::FrameGrabber::Ptr fg_{}; ifm3d::FrameGrabber::Ptr fg_diag_{}; + // Holds the list of buffers that are provided to the framegrabber + std::vector> buffer_list_{}; + + std::variant, std::shared_ptr> data_module_; + std::shared_ptr diag_module_; + std::vector> modules_; - /// global mutex on ifm3d core data structures `cam_`, `fg_` - std::mutex gil_{}; + /// global mutex on ifm3d core data structures `o3r_`, `fg_` + std::shared_ptr gil_{}; - /// Differentiation between 2D and 3D data stream, derived from ifm3d::O3R cam_ + /// Differentiation between 2D and 3D data stream, derived from ifm3d::O3R o3r_ ifm3d_ros2::buffer_id_utils::data_stream_type data_stream_type_; // Values read from parameters - std::vector buffer_id_list_{}; + std::string config_file_{}; std::string ip_{}; std::uint16_t pcic_port_{}; - std::string tf_cloud_link_frame_name_{}; - bool tf_cloud_link_publish_transform_{}; - std::string tf_mounting_link_frame_name_{}; - std::string tf_optical_link_frame_name_{}; - bool tf_optical_link_publish_transform_{}; - std::vector tf_optical_link_transform_{}; std::uint16_t xmlrpc_port_{}; - std::string diag_mode_{}; - // Values read from incomming image buffers - uint32_t width_; - uint32_t height_; + // For backward compatibility, we get the port name + // from the provided pcic_port. + ifm3d::PortInfo port_info_{}; /// Subscription to parameter changes std::shared_ptr param_subscriber_; /// Callbacks need to be stored to work properly; using a map with parameter name as key std::map registered_param_callbacks_; - // TF handling - std::shared_ptr tf_static_broadcaster_; - geometry_msgs::msg::TransformStamped cloud_link_transform_; - // Service Servers - DumpServer dump_srv_{}; - ConfigServer config_srv_{}; - SoftoffServer soft_off_srv_{}; - SoftonServer soft_on_srv_{}; - - // Publishers - DiagnosticArrayPublisher diagnostic_publisher_; - std::map image_publishers_; - std::map compressed_image_publishers_; - std::map pcl_publishers_; - std::map extrinsics_publishers_; - std::map camera_info_publishers_; - std::map rgb_info_publishers_; - std::map tof_info_publishers_; - std::map intrinsics_publishers_; - std::map inverse_intrinsics_publishers_; - - // Timer for publishing diagnostics - rclcpp::TimerBase::SharedPtr diagnostic_timer_; + std::shared_ptr base_services_{}; }; // end: class CameraNode diff --git a/include/ifm3d_ros2/camera_tf_publisher.hpp b/include/ifm3d_ros2/camera_tf_publisher.hpp new file mode 100644 index 0000000..5e5507d --- /dev/null +++ b/include/ifm3d_ros2/camera_tf_publisher.hpp @@ -0,0 +1,57 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#ifndef IFM3D_ROS2_CAMERA_TF_PUBLISHER_HPP_ +#define IFM3D_ROS2_CAMERA_TF_PUBLISHER_HPP_ + +#include +#include +#include +#include +#include + +namespace ifm3d_ros2 +{ +class CameraTfPublisher +{ +public: + CameraTfPublisher(rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr, ifm3d::O3R::Ptr o3r_ptr, std::string port, + std::string base_frame_name, std::string mounting_frame_name, std::string optical_frame_name); + CameraTfPublisher(rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr, ifm3d::O3R::Ptr o3r_ptr, std::string port); + + bool update_and_publish_tf_if_changed(const geometry_msgs::msg::TransformStamped& new_tf_base_to_optical); + + std::string tf_to_string(geometry_msgs::msg::TransformStamped tf); + + std::string tf_base_link_frame_name_; + std::string tf_mounting_link_frame_name_; + std::string tf_optical_link_frame_name_; + bool tf_publish_mounting_to_optical_; + bool tf_publish_base_to_mounting_; + +protected: + bool transform_identical(geometry_msgs::msg::TransformStamped tf1, geometry_msgs::msg::TransformStamped tf2); + + geometry_msgs::msg::TransformStamped read_tf_base_to_mounting_from_device_config(builtin_interfaces::msg::Time stamp); + + geometry_msgs::msg::TransformStamped get_tf_mounting_to_optical( + builtin_interfaces::msg::Time stamp, geometry_msgs::msg::TransformStamped tf_base_to_optical, + geometry_msgs::msg::TransformStamped tf_base_to_mounting); + + geometry_msgs::msg::TransformStamped calculate_tf_base_to_optical( + builtin_interfaces::msg::Time stamp, geometry_msgs::msg::TransformStamped tf_base_to_mounting, + geometry_msgs::msg::TransformStamped tf_mounting_to_optical); + +private: + rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr_; + ifm3d::O3R::Ptr o3r_ptr_; + std::string port_; + std::shared_ptr tf_static_broadcaster_; + geometry_msgs::msg::TransformStamped tf_base_to_optical_, tf_base_to_mounting_, tf_mounting_to_optical_; +}; + +} // namespace ifm3d_ros2 +#endif \ No newline at end of file diff --git a/include/ifm3d_ros2/diag_module.hpp b/include/ifm3d_ros2/diag_module.hpp new file mode 100644 index 0000000..fd876e3 --- /dev/null +++ b/include/ifm3d_ros2/diag_module.hpp @@ -0,0 +1,93 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#ifndef IFM3D_ROS2_DIAG_MODULE_HPP_ +#define IFM3D_ROS2_DIAG_MODULE_HPP_ + +#include +#include +#include +#include +#include + +namespace ifm3d_ros2 +{ +class DiagModule : public FunctionModule, public std::enable_shared_from_this +{ + using DiagnosticStatusMsg = diagnostic_msgs::msg::DiagnosticStatus; + using DiagnosticArrayMsg = diagnostic_msgs::msg::DiagnosticArray; + using DiagnosticArrayPublisher = std::shared_ptr>; + +public: + DiagModule(rclcpp::Logger logger, rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr, + std::shared_ptr o3r); + /** + * @brief Main function that handles diagnostic messages. + * When an error is received, it is directly published to the /diagnostic topic. + * + * @param i Error code + * @param s Error description in JSON format + */ + void handle_error(int i, const std::string& s); + /** + * @brief Main function that handles notifications. + * When a notification is received, it is directly published to the /diagnostic topic + * with the level OK. + * + * @param s1 Notification id + * @param s2 Notification description in JSON + */ + void handle_notification(const std::string& s1, const std::string& s2); + + /** + * @brief Callback called every second, as defined by the diagnostic_timer_. + * It formats and publishes the list of diagnostic messages coming from the device. + * + */ + void periodic_diag_callback(); + + /** + * Utility functions to creates a DiagnosticStatusMsg and a DiagnosticArrayMsg + * from the received JSON string. + * + */ + DiagnosticStatusMsg create_diagnostic_status(const uint8_t level, ifm3d::json parsed_json); + DiagnosticArrayMsg create_diagnostic_message(const uint8_t level, const std::string& json_msg); + + const std::string get_name() + { + return "diag_module"; + }; + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_error(const rclcpp_lifecycle::State& previous_state); + +private: + /// Pointer to the shared O3R object to access diagnostic using the GetDiagnostic methods. + std::shared_ptr o3r_; + + /// Pointer to the node to access the ROS2 API. + rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr_; + + rclcpp::TimerBase::SharedPtr diagnostic_timer_; + + DiagnosticArrayPublisher diagnostic_publisher_; + /// A unique identifier for the diagnostic messages. + std::string hardware_id_; +}; + +} // namespace ifm3d_ros2 + +#endif \ No newline at end of file diff --git a/include/ifm3d_ros2/function_module.hpp b/include/ifm3d_ros2/function_module.hpp new file mode 100644 index 0000000..588b55f --- /dev/null +++ b/include/ifm3d_ros2/function_module.hpp @@ -0,0 +1,57 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#ifndef IFM3D_ROS2_FUNCTION_MODULE_HPP_ +#define IFM3D_ROS2_FUNCTION_MODULE_HPP_ + +#include + +#include +#include +#include +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +namespace ifm3d_ros2 +{ +/** + * @brief Abstract representation of encapsulated camera functionality. + * + * Wraps a set of sub-functions of a camera. + * Follows the lifecycle of rclcpp_lifecycle. + */ +class FunctionModule : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface, + public std::enable_shared_from_this +{ +public: + FunctionModule(const rclcpp::Logger& logger); + + virtual void handle_frame(ifm3d::Frame::Ptr frame); + + virtual const std::string get_name() = 0; + + virtual rclcpp_lifecycle::LifecycleNode::CallbackReturn + on_configure(const rclcpp_lifecycle::State& previous_state) = 0; + + virtual rclcpp_lifecycle::LifecycleNode::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) = 0; + + virtual rclcpp_lifecycle::LifecycleNode::CallbackReturn + on_shutdown(const rclcpp_lifecycle::State& previous_state) = 0; + + virtual rclcpp_lifecycle::LifecycleNode::CallbackReturn + on_activate(const rclcpp_lifecycle::State& previous_state) = 0; + + virtual rclcpp_lifecycle::LifecycleNode::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State& previous_state) = 0; + + virtual rclcpp_lifecycle::LifecycleNode::CallbackReturn on_error(const rclcpp_lifecycle::State& previous_state) = 0; + +protected: + rclcpp::Logger logger_; +}; + +} // namespace ifm3d_ros2 + +#endif \ No newline at end of file diff --git a/include/ifm3d_ros2/ods_module.hpp b/include/ifm3d_ros2/ods_module.hpp new file mode 100644 index 0000000..6dda50a --- /dev/null +++ b/include/ifm3d_ros2/ods_module.hpp @@ -0,0 +1,78 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#ifndef IFM3D_ROS2_ODS_MODULE_HPP_ +#define IFM3D_ROS2_ODS_MODULE_HPP_ + +#include +#include +#include + +#include +#include + +#include + +namespace ifm3d_ros2 +{ +/** + * @brief Wraps the ODS application. + */ +class OdsModule : public FunctionModule, public std::enable_shared_from_this +{ + using CostmapMsg = nav2_msgs::msg::Costmap; + using CostmapPublisher = std::shared_ptr>; + + using OccupancyGridMsg = nav_msgs::msg::OccupancyGrid; + using OccupancyGridPublisher = std::shared_ptr>; + + using ZonesMsg = ifm3d_ros2::msg::Zones; + using ZonesPublisher = std::shared_ptr>; + +public: + OdsModule(rclcpp::Logger logger, rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr); + // Main functions that take care of deserializing + // and publishing the ODS data + void handle_frame(ifm3d::Frame::Ptr frame); + nav_msgs::msg::OccupancyGrid extract_ros_occupancy_grid(ifm3d::Frame::Ptr frame); + nav2_msgs::msg::Costmap extract_ros_costmap(ifm3d::Frame::Ptr frame); + ifm3d_ros2::msg::Zones extract_zones(ifm3d::Frame::Ptr frame); + + const std::string get_name() + { + return "ods_module"; + }; + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_error(const rclcpp_lifecycle::State& previous_state); + +private: + rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr_; + + OccupancyGridPublisher ros_occupancy_grid_publisher_; + CostmapPublisher ros_costmap_publisher_; + ZonesPublisher zones_publisher_; + + std::string frame_id_; + bool publish_occupancy_grid_; + bool publish_costmap_; + rcl_interfaces::msg::ParameterDescriptor frame_id_descriptor_; + rcl_interfaces::msg::ParameterDescriptor publish_occupancy_grid_descriptor_; + rcl_interfaces::msg::ParameterDescriptor publish_costmap_descriptor_; +}; + +} // namespace ifm3d_ros2 + +#endif \ No newline at end of file diff --git a/include/ifm3d_ros2/ods_node.hpp b/include/ifm3d_ros2/ods_node.hpp new file mode 100644 index 0000000..7cdb075 --- /dev/null +++ b/include/ifm3d_ros2/ods_node.hpp @@ -0,0 +1,206 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#ifndef IFM3D_ROS2_ODS_NODE_HPP_ +#define IFM3D_ROS2_ODS_NODE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +using TC_RETVAL = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +namespace ifm3d_ros2 +{ +/** + * Managed node that implements the ODS application of ifm3d for ROS 2. + */ +class IFM3D_ROS2_PUBLIC OdsNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + /** + * Instantiates the LifecycleNode. At the completion of the ctor, the + * following initialization (beyond calling the parent ctor) has been done: + * + * - A named logger for this node has been initialized + * - tf frame names have been initialzed based on the node name + * - All parameters have been declared and a `set` callback has been + * registered + * - All publishers have been created. + */ + explicit OdsNode(const std::string& node_name, const rclcpp::NodeOptions& opts); + + /** + * Delegates construction to the above ctor. + */ + explicit OdsNode(const rclcpp::NodeOptions& opts); + + /** + * RAII deallocations. As of this writing, given that all structures are + * handled by various types of smart pointers no "real work" needs to be + * done here. However, for debugging purposes we emit a log message so we + * know when the dtor has actually been called and hence when all + * deallocations actually occur. + */ + ~OdsNode() override; + + /** + * Implements the "configuring" transition state + * + * The following operations are performed: + * + * - Parameters are parsed and held locally in instance variables. + * - If requested, the camera clock is synchronized to the system clock + * - The core ifm3d data structures (camera, framegrabber, stlimage buffer) + * are initialized and ready to stream data based upon the requested + * schema mask. + */ + TC_RETVAL on_configure(const rclcpp_lifecycle::State& prev_state) override; + + /** + * Implements the "activating" transition state + * + * The following operations are performed: + * + * - The `on_activate()` method is called on all publishers + * - A new thread is started that will continuous publish image data from + * the camera. + */ + TC_RETVAL on_activate(const rclcpp_lifecycle::State& prev_state) override; + + /** + * Implements the "deactivating" transition state + * + * The following operations are performed: + * + * - The thread that implements the "publish loop" is stopped + * - All publishers can their `on_deactivate()` method called + */ + TC_RETVAL on_deactivate(const rclcpp_lifecycle::State& prev_state) override; + + /** + * Implements the "cleaningup" transition state + * + * The following operations are performed: + * + * - The ifm3d core data structures (camera, framegrabber, stlimage buffer) + * have their dtors called + */ + TC_RETVAL on_cleanup(const rclcpp_lifecycle::State& prev_state) override; + + /** + * Implements the "shuttingdown" transition state + * + * The following operations are performed: + * + * - It is ensured that the publishing loop thread is stopped + */ + TC_RETVAL on_shutdown(const rclcpp_lifecycle::State& prev_state) override; + + /** + * Implements the "errorprocessing" transition state + * + * The following operations are performed: + * + * - The publish_loop thread is stopped (if running) + * - The ifm3d core data structures (camera, framegrabber, stlimage buffer) + * have their dtors called + */ + TC_RETVAL on_error(const rclcpp_lifecycle::State& prev_state) override; + +protected: + /** + * Declares parameters and default values + */ + void init_params(); + + /** + * Reads parameters, needs init_params() to be called beforehand + */ + void parse_params(); + + /** + * Sets the callbacks handling parameter changes at runtime + */ + void set_parameter_event_callbacks(); + + /** + * Callback which receives new Frames from ifm3d + */ + void frame_callback(ifm3d::Frame::Ptr frame); + + /** + * Callback which receives Errors from ifm3d + */ + void error_callback(const ifm3d::Error& error); + + /** + * Callback which receives AsyncErrors from ifm3d + */ + void async_error_callback(int i, const std::string& s); + + /** + * Callback which receives AsyncNotifications from ifm3d + */ + void async_notification_callback(const std::string& s1, const std::string& s2); + +private: + rclcpp::Logger logger_; + + ifm3d::O3R::Ptr o3r_{}; // TODO probably need some other device + ifm3d::FrameGrabber::Ptr fg_{}; + ifm3d::FrameGrabber::Ptr fg_diag_{}; + + std::shared_ptr ods_module_; + std::shared_ptr diag_module_; + std::vector> modules_; + + /// global mutex on ifm3d core data structures `fg_` + std::shared_ptr gil_{}; + + // Values read from parameters + std::string config_file_{}; + std::string ip_{}; + std::uint16_t pcic_port_{}; + std::uint16_t xmlrpc_port_{}; + + ifm3d::PortInfo port_info_{}; + + // Values read from incoming image buffers + uint32_t width_; + uint32_t height_; + + /// Subscription to parameter changes + std::shared_ptr param_subscriber_; + /// Callbacks need to be stored to work properly; using a map with parameter name as key + std::map registered_param_callbacks_; + + // Service Servers + std::shared_ptr base_services_{}; + +}; // end: class OdsNode + +} // namespace ifm3d_ros2 + +#endif // IFM3D_ROS2_ODS_NODE_HPP_ diff --git a/include/ifm3d_ros2/qos.hpp b/include/ifm3d_ros2/qos.hpp index aa4dcea..654e588 100644 --- a/include/ifm3d_ros2/qos.hpp +++ b/include/ifm3d_ros2/qos.hpp @@ -1,7 +1,7 @@ // -*- c++ -*- /* * SPDX-License-Identifier: Apache-2.0 - * Copyright (C) 2019 ifm electronic, gmbh + * Copyright (C) 2024 ifm electronic, gmbh */ #ifndef IFM3D_ROS2_QOS_HPP_ diff --git a/include/ifm3d_ros2/rgb_module.hpp b/include/ifm3d_ros2/rgb_module.hpp new file mode 100644 index 0000000..f77daf9 --- /dev/null +++ b/include/ifm3d_ros2/rgb_module.hpp @@ -0,0 +1,101 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#ifndef IFM3D_ROS2_RGB_MODULE_HPP_ +#define IFM3D_ROS2_RGB_MODULE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +using CompressedImageMsg = sensor_msgs::msg::CompressedImage; +using CompressedImagePublisher = std::shared_ptr>; + +using RGBInfoMsg = ifm3d_ros2::msg::RGBInfo; +using RGBInfoPublisher = std::shared_ptr>; + +using CameraInfoMsg = sensor_msgs::msg::CameraInfo; +using CameraInfoPublisher = std::shared_ptr>; + +namespace ifm3d_ros2 +{ +class RgbModule : public FunctionModule, public std::enable_shared_from_this +{ +public: + RgbModule(rclcpp::Logger logger, rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr, ifm3d::O3R::Ptr o3r_ptr, + std::string port, uint32_t width, uint32_t height); + void handle_frame(ifm3d::Frame::Ptr frame); + + const std::string get_name() + { + return "rgb_module"; + }; + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_error(const rclcpp_lifecycle::State& previous_state); + + // The list is accessed in the node, to start the FrameGrabber + std::vector buffer_id_list_{}; + +protected: + void parse_params(); + void set_parameter_event_callbacks(); + +private: + rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr_; + CameraTfPublisher tf_publisher_; + + // Values read from incoming image buffers + uint32_t width_; + uint32_t height_; + + // Boolean to track first time publishing, so that we only + // publish the static transforms once + bool first_; + + // Publishers + CompressedImagePublisher rgb_publisher_; + RGBInfoPublisher rgb_info_publisher_; + CameraInfoPublisher camera_info_publisher_; + + // Parameters + rcl_interfaces::msg::ParameterDescriptor buffer_id_list_descriptor_; + rcl_interfaces::msg::ParameterDescriptor tf_base_frame_name_descriptor_; + rcl_interfaces::msg::ParameterDescriptor tf_mounting_frame_name_descriptor_; + rcl_interfaces::msg::ParameterDescriptor tf_optical_frame_name_descriptor_; + rcl_interfaces::msg::ParameterDescriptor tf_publish_mounting_to_optical_descriptor_; + rcl_interfaces::msg::ParameterDescriptor tf_publish_base_to_mounting_descriptor_; + + // TF handling + + /// Subscription to parameter changes + std::shared_ptr param_subscriber_; + /// Callbacks need to be stored to work properly; using a map with parameter name as key + std::map registered_param_callbacks_; +}; + +} // namespace ifm3d_ros2 + +#endif \ No newline at end of file diff --git a/include/ifm3d_ros2/services.hpp b/include/ifm3d_ros2/services.hpp new file mode 100644 index 0000000..fadb12e --- /dev/null +++ b/include/ifm3d_ros2/services.hpp @@ -0,0 +1,88 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#ifndef IFM3D_ROS2_SERVICES_HPP_ +#define IFM3D_ROS2_SERVICES_HPP_ + +#include + +#include +#include +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +using DumpRequest = std::shared_ptr; +using DumpResponse = std::shared_ptr; +using DumpService = ifm3d_ros2::srv::Dump; +using DumpServer = rclcpp::Service::SharedPtr; + +using ConfigRequest = std::shared_ptr; +using ConfigResponse = std::shared_ptr; +using ConfigService = ifm3d_ros2::srv::Config; +using ConfigServer = rclcpp::Service::SharedPtr; + +using SoftoffRequest = std::shared_ptr; +using SoftoffResponse = std::shared_ptr; +using SoftoffService = ifm3d_ros2::srv::Softoff; +using SoftoffServer = rclcpp::Service::SharedPtr; + +using SoftonRequest = std::shared_ptr; +using SoftonResponse = std::shared_ptr; +using SoftonService = ifm3d_ros2::srv::Softon; +using SoftonServer = rclcpp::Service::SharedPtr; + +using GetDiagRequest = std::shared_ptr; +using GetDiagResponse = std::shared_ptr; +using GetDiagService = ifm3d_ros2::srv::GetDiag; +using GetDiagServer = rclcpp::Service::SharedPtr; + +namespace ifm3d_ros2 +{ +class BaseServices : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface, + public std::enable_shared_from_this +{ +public: + BaseServices(rclcpp::Logger logger, rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr, ifm3d::O3R::Ptr cam, + ifm3d::PortInfo port_info, std::shared_ptr ifm3d_mutex); + +protected: + void Dump(std::shared_ptr request_header, DumpRequest req, DumpResponse resp); + void Config(std::shared_ptr request_header, ConfigRequest req, ConfigResponse resp); + void Softoff(std::shared_ptr request_header, SoftoffRequest req, SoftoffResponse resp); + void Softon(std::shared_ptr request_header, SoftonRequest req, SoftonResponse resp); + void GetDiag(std::shared_ptr request_header, GetDiagRequest req, GetDiagResponse resp); + + rclcpp::Logger logger_; + +private: + // Need a pointer to node to create services + rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr_; + + /// global mutex on ifm3d core data structures `fg_` + std::shared_ptr ifm3d_mutex_{}; + + ifm3d::O3R::Ptr cam_{}; + ifm3d::PortInfo port_info_{}; + + // Service Servers + DumpServer dump_srv_{}; + ConfigServer config_srv_{}; + SoftoffServer soft_off_srv_{}; + SoftonServer soft_on_srv_{}; + GetDiagServer get_diag_srv_{}; +}; + +} // namespace ifm3d_ros2 + +#endif // IFM3D_ROS2_BUFFER_CONVERSIONS_HPP_ \ No newline at end of file diff --git a/include/ifm3d_ros2/tof_module.hpp b/include/ifm3d_ros2/tof_module.hpp new file mode 100644 index 0000000..f64ce72 --- /dev/null +++ b/include/ifm3d_ros2/tof_module.hpp @@ -0,0 +1,127 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#ifndef IFM3D_ROS2_TOF_MODULE_HPP_ +#define IFM3D_ROS2_TOF_MODULE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using ImageMsg = sensor_msgs::msg::Image; +using ImagePublisher = std::shared_ptr>; + +using PCLMsg = sensor_msgs::msg::PointCloud2; +using PCLPublisher = std::shared_ptr>; + +using ExtrinsicsMsg = ifm3d_ros2::msg::Extrinsics; +using ExtrinsicsPublisher = std::shared_ptr>; + +using CameraInfoMsg = sensor_msgs::msg::CameraInfo; +using CameraInfoPublisher = std::shared_ptr>; + +using IntrinsicsMsg = ifm3d_ros2::msg::Intrinsics; +using IntrinsicsPublisher = std::shared_ptr>; + +using InverseIntrinsicsMsg = ifm3d_ros2::msg::InverseIntrinsics; +using InverseIntrinsicsPublisher = std::shared_ptr>; + +using TOFInfoMsg = ifm3d_ros2::msg::TOFInfo; +using TOFInfoPublisher = std::shared_ptr>; + +namespace ifm3d_ros2 +{ +class TofModule : public FunctionModule, public std::enable_shared_from_this +{ +public: + TofModule(rclcpp::Logger logger, rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr, ifm3d::O3R::Ptr o3r_ptr, + std::string port, uint32_t width, uint32_t height); + /** + * @brief Unpacks data from the received frame, and publish + * the topics corresponding to the requested image buffers. + * + * @param frame + */ + + void handle_frame(ifm3d::Frame::Ptr frame); + + const std::string get_name() + { + return "tof_module"; + }; + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state); + + rclcpp_lifecycle::LifecycleNode::CallbackReturn on_error(const rclcpp_lifecycle::State& previous_state); + + // The list is accessed in the node, to start the FrameGrabber + std::vector buffer_id_list_{}; + +protected: + void parse_params(); + void set_parameter_event_callbacks(); + +private: + rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr_; + CameraTfPublisher tf_publisher_; + + // Values read from incoming image buffers + uint32_t width_; + uint32_t height_; + + // Value used to only publish the static transform once + bool first_; + + // Publishers + // We create lists of publishers for same image types, instead + // of creating a publisher for each buffer. This way we limit the + // number of publishers to the number of image types. + std::map image_publishers_; + std::map pcl_publishers_; + std::map extrinsics_publishers_; + std::map camera_info_publishers_; + std::map tof_info_publishers_; + std::map intrinsics_publishers_; + std::map inverse_intrinsics_publishers_; + + // Parameters + rcl_interfaces::msg::ParameterDescriptor buffer_id_list_descriptor_; + rcl_interfaces::msg::ParameterDescriptor tf_base_frame_name_descriptor_; + rcl_interfaces::msg::ParameterDescriptor tf_mounting_frame_name_descriptor_; + rcl_interfaces::msg::ParameterDescriptor tf_optical_frame_name_descriptor_; + rcl_interfaces::msg::ParameterDescriptor tf_publish_mounting_to_optical_descriptor_; + rcl_interfaces::msg::ParameterDescriptor tf_publish_base_to_mounting_descriptor_; + + /// Subscription to parameter changes + std::shared_ptr param_subscriber_; + /// Callbacks need to be stored to work properly; using a map with parameter name as key + std::map registered_param_callbacks_; +}; + +} // namespace ifm3d_ros2 + +#endif \ No newline at end of file diff --git a/include/ifm3d_ros2/visibility_control.h b/include/ifm3d_ros2/visibility_control.h index 9b25cb5..52a1e0c 100644 --- a/include/ifm3d_ros2/visibility_control.h +++ b/include/ifm3d_ros2/visibility_control.h @@ -17,39 +17,38 @@ #define IFM3D_ROS2__VISIBILITY_CONTROL_H_ #ifdef __cplusplus -extern "C" -{ +extern "C" { #endif // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define IFM3D_ROS2_EXPORT __attribute__ ((dllexport)) - #define IFM3D_ROS2_IMPORT __attribute__ ((dllimport)) - #else - #define IFM3D_ROS2_EXPORT __declspec(dllexport) - #define IFM3D_ROS2_IMPORT __declspec(dllimport) - #endif - #ifdef IFM3D_ROS2_BUILDING_DLL - #define IFM3D_ROS2_PUBLIC IFM3D_ROS2_EXPORT - #else - #define IFM3D_ROS2_PUBLIC IFM3D_ROS2_IMPORT - #endif - #define IFM3D_ROS2_PUBLIC_TYPE IFM3D_ROS2_PUBLIC - #define IFM3D_ROS2_LOCAL +#ifdef __GNUC__ +#define IFM3D_ROS2_EXPORT __attribute__((dllexport)) +#define IFM3D_ROS2_IMPORT __attribute__((dllimport)) #else - #define IFM3D_ROS2_EXPORT __attribute__ ((visibility("default"))) - #define IFM3D_ROS2_IMPORT - #if __GNUC__ >= 4 - #define IFM3D_ROS2_PUBLIC __attribute__ ((visibility("default"))) - #define IFM3D_ROS2_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define IFM3D_ROS2_PUBLIC - #define IFM3D_ROS2_LOCAL - #endif - #define IFM3D_ROS2_PUBLIC_TYPE +#define IFM3D_ROS2_EXPORT __declspec(dllexport) +#define IFM3D_ROS2_IMPORT __declspec(dllimport) +#endif +#ifdef IFM3D_ROS2_BUILDING_DLL +#define IFM3D_ROS2_PUBLIC IFM3D_ROS2_EXPORT +#else +#define IFM3D_ROS2_PUBLIC IFM3D_ROS2_IMPORT +#endif +#define IFM3D_ROS2_PUBLIC_TYPE IFM3D_ROS2_PUBLIC +#define IFM3D_ROS2_LOCAL +#else +#define IFM3D_ROS2_EXPORT __attribute__((visibility("default"))) +#define IFM3D_ROS2_IMPORT +#if __GNUC__ >= 4 +#define IFM3D_ROS2_PUBLIC __attribute__((visibility("default"))) +#define IFM3D_ROS2_LOCAL __attribute__((visibility("hidden"))) +#else +#define IFM3D_ROS2_PUBLIC +#define IFM3D_ROS2_LOCAL +#endif +#define IFM3D_ROS2_PUBLIC_TYPE #endif #ifdef __cplusplus diff --git a/index.md b/index.md index b72f116..1864ab9 100644 --- a/index.md +++ b/index.md @@ -3,12 +3,10 @@ :maxdepth: 2 Overview Installation -Launch +Camera node +ODS node Visualization -Topics Services -Parameters -Multi-head -Diagnostic +Diagnostic Deployment ::: \ No newline at end of file diff --git a/launch/camera.launch.py b/launch/camera.launch.py index 2dde24a..1bce6ef 100644 --- a/launch/camera.launch.py +++ b/launch/camera.launch.py @@ -1,6 +1,6 @@ # # SPDX-License-Identifier: Apache-2.0 -# Copyright (C) 2032 ifm electronic, gmbh +# Copyright (C) 2024 ifm electronic, gmbh # from launch import LaunchDescription diff --git a/launch/camera_managed.launch.py b/launch/camera_managed.launch.py deleted file mode 100644 index 4f18ef3..0000000 --- a/launch/camera_managed.launch.py +++ /dev/null @@ -1,241 +0,0 @@ -# -# SPDX-License-Identifier: Apache-2.0 -# Copyright (C) 2019 ifm electronic, gmbh -# - - - -import os -from math import pi -import logging - -import lifecycle_msgs.msg -import launch -from launch import LaunchDescription -from launch.actions import ExecuteProcess -from launch.actions import EmitEvent -from launch.actions import LogInfo -from launch.actions import RegisterEventHandler -from launch_ros.actions import LifecycleNode -from launch_ros.events.lifecycle import ChangeState -from launch_ros.event_handlers import OnStateTransition - -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch.actions import OpaqueFunction - -deprecation_warning = LogInfo( - msg=""" - - ###################################################################################### - # # - # This launch script is deprecated. Use the parametrized camera.launch.py instead! # - # # - ###################################################################################### - """ -) - -logging.basicConfig(format='%(asctime)s - %(message)s', level=logging.INFO) - -def launch_setup(context, *args, **kwargs): - - - package_name = 'ifm3d_ros2' - node_exe = 'camera_standalone' - parameters = [] - remaps = [] - - node_name = LaunchConfiguration('name').perform(context) - params = LaunchConfiguration('params').perform(context) - node_namespace = LaunchConfiguration('namespace').perform(context) - - parameters.append(params) - - #------------------------------------------------------------ - # Nodes - #------------------------------------------------------------ - - - # - # The camera component - # - camera_node = \ - LifecycleNode( - package=package_name, - executable=node_exe, - namespace=node_namespace, - name=node_name, - output='screen', - parameters=parameters, - remappings=remaps, - log_cmd=True, - ) - - logging.debug(vars(camera_node)) - #------------------------------------------------------------ - # Events we need to emit to induce state transitions - #------------------------------------------------------------ - - camera_configure_evt = \ - EmitEvent( - event=ChangeState( - lifecycle_node_matcher = \ - launch.events.matches_action(camera_node), - transition_id = lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE - ) - ) - - camera_activate_evt = \ - EmitEvent( - event=ChangeState( - lifecycle_node_matcher = \ - launch.events.matches_action(camera_node), - transition_id = lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE - ) - ) - - camera_cleanup_evt = \ - EmitEvent( - event=ChangeState( - lifecycle_node_matcher = \ - launch.events.matches_action(camera_node), - transition_id = lifecycle_msgs.msg.Transition.TRANSITION_CLEANUP - ) - ) - - camera_shutdown_evt = EmitEvent(event=launch.events.Shutdown()) - - #------------------------------------------------------------ - # These are the edges of the state machine graph we want to autonomously - # manage - #------------------------------------------------------------ - - # - # unconfigured -> configuring -> inactive - # - camera_node_unconfigured_to_inactive_handler = \ - RegisterEventHandler( - OnStateTransition( - target_lifecycle_node = camera_node, - start_state = 'configuring', - goal_state = 'inactive', - entities = [ - LogInfo(msg = "Emitting 'TRANSITION_ACTIVATE' event"), - camera_activate_evt, - ], - ) - ) - # - # active -> deactivating -> inactive - # - camera_node_active_to_inactive_handler = \ - RegisterEventHandler( - OnStateTransition( - target_lifecycle_node = camera_node, - start_state = 'deactivating', - goal_state = 'inactive', - entities = [ - LogInfo(msg = "Emitting 'TRANSITION_CLEANUP' event"), - camera_cleanup_evt, - ], - ) - ) - # - # inactive -> cleaningup -> unconfigured - # - camera_node_inactive_to_unconfigured_handler = \ - RegisterEventHandler( - OnStateTransition( - target_lifecycle_node = camera_node, - start_state = 'cleaningup', - goal_state = 'unconfigured', - entities = [ - LogInfo(msg = "Emitting 'TRANSITION_CONFIGURE' event"), - camera_configure_evt, - ], - ) - ) - # - # * -> errorprocessing -> unconfigured - # - camera_node_errorprocessing_to_unconfigured_handler = \ - RegisterEventHandler( - OnStateTransition( - target_lifecycle_node = camera_node, - start_state = 'errorprocessing', - goal_state = 'unconfigured', - entities = [ - LogInfo(msg = "Emitting 'TRANSITION_CONFIGURE' event"), - camera_configure_evt, - ], - ) - ) - # - # * -> shuttingdown -> finalized - # - camera_node_shuttingdown_to_finalized_handler = \ - RegisterEventHandler( - OnStateTransition( - target_lifecycle_node = camera_node, - start_state = 'shuttingdown', - goal_state = 'finalized', - entities = [ - LogInfo(msg = "Emitting 'SHUTDOWN' event"), - camera_shutdown_evt, - ], - ) - ) - - # - # Coord frame transform from camera_optical_link to camera_link - # - tf_node = \ - ExecuteProcess( - cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', - '0', '0', '0', '0', '0', '0', - str(node_name + "_optical_link"), str(node_name + "_link")], - # output='screen', - log_cmd=True - ) - logging.info("Publishing tf2 transform from {} to {}" .format(str(node_name + "_optical_link"), str(node_name + "_link"))) - - # - # (Dummy) Coord frame transform from camera_link to map frame - # - tf_map_link_node = \ - ExecuteProcess( - cmd=['ros2', 'run', 'tf2_ros', 'static_transform_publisher', - '0', '0', '0', '0', '0', '0', - "map", str(node_name + "_optical_link")], - # output='screen', - log_cmd=True - ) - logging.info("Publishing tf2 transform from {} to {}" .format("map", str(node_name + "_optical_link"))) - - return camera_node_unconfigured_to_inactive_handler, \ - camera_node_active_to_inactive_handler, \ - camera_node_inactive_to_unconfigured_handler, \ - camera_node_errorprocessing_to_unconfigured_handler, \ - camera_node_shuttingdown_to_finalized_handler, \ - camera_node, \ - camera_configure_evt, \ - tf_node, \ - tf_map_link_node \ - - -def generate_launch_description(): - - os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = \ - "[{%s}] {%s} [{%s}]: {%s}" % ("severity", "time", "name", "message") - - - return LaunchDescription( - [ - deprecation_warning, - DeclareLaunchArgument('name', default_value='camera'), - DeclareLaunchArgument('params', default_value=[]), - DeclareLaunchArgument('namespace', default_value='ifm3d'), - OpaqueFunction(function=launch_setup), - ], - deprecated_reason="Deprecated in favor of camera.launch.py" - ) diff --git a/launch/camera_standalone.launch.py b/launch/camera_standalone.launch.py deleted file mode 100644 index 975561e..0000000 --- a/launch/camera_standalone.launch.py +++ /dev/null @@ -1,123 +0,0 @@ -# -# SPDX-License-Identifier: Apache-2.0 -# Copyright (C) 2019 ifm electronic, gmbh -# - -import os -import sys -from math import pi - -from launch import LaunchDescription -from launch.actions import ExecuteProcess, LogInfo -from launch_ros.actions import LifecycleNode - -deprecation_warning = LogInfo( - msg=""" - - ###################################################################################### - # # - # This launch script is deprecated. Use the parametrized camera.launch.py instead! # - # # - ###################################################################################### - """ -) - - -def generate_launch_description(): - package_name = 'ifm3d_ros2' - node_namespace = 'ifm3d' - node_name = 'camera' - node_exe = 'camera_standalone' - - # os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = \ - # "[{%s}] {%s} [{%s}]: {%s}\n({%s}() at {%s}:{%s})" % \ - # ("severity", "time", "name", "message", - # "function_name", "file_name", "line_number") - os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = \ - "[{%s}] {%s} [{%s}]: {%s}" % ("severity", "time", "name", "message") - - # XXX: This is a hack, there does not seem to be a nice way (or at least - # finding the docs is not obvious) to do this with the ROS2 launch api - # - # Basically, we are trying to allow for passing through the command line - # args to the launch file through to the node executable itself (like ROS - # 1). - # - # My assumption is that either: - # 1. This stuff exists somewhere in ROS2 and I don't know about it yet - # 2. This stuff will exist in ROS2 soon, so, this will likely get factored - # out (hopefully soon) - # - parameters = [] - remaps = [] - for arg in sys.argv: - if ':=' in arg: - split_arg = arg.split(sep=':=', maxsplit=1) - assert len(split_arg) == 2 - - if arg.startswith("ns"): - node_namespace = split_arg[1] - elif arg.startswith("node"): - node_name = split_arg[1] - elif arg.startswith("params"): - parameters.append(tuple(split_arg)[1]) - else: - remaps.append(tuple(split_arg)) - - def add_prefix(tup): - assert len(tup) == 2 - if node_namespace.startswith("/"): - prefix = "%s/%s" % (node_namespace, node_name) - else: - prefix = "/%s/%s" % (node_namespace, node_name) - - retval = [None, None] - - if not tup[0].startswith(prefix): - retval[0] = prefix + '/' + tup[0] - else: - retval[0] = tup[0] - - if not tup[1].startswith(prefix): - retval[1] = prefix + '/' + tup[1] - else: - retval[1] = tup[1] - - return tuple(retval) - - remaps = list(map(add_prefix, remaps)) - - return LaunchDescription( - [ - deprecation_warning, - ExecuteProcess( - cmd=[ - 'ros2', - 'run', - 'tf2_ros', - 'static_transform_publisher', - '0', - '0', - '0', - '0', - '0', - '0', - str(node_name + "_optical_link"), - str(node_name + "_link"), - ], - # output='screen', - log_cmd=True, - ), - LifecycleNode( - package=package_name, - executable=node_exe, - namespace=node_namespace, - name=node_name, - output='screen', - parameters=parameters, - remappings=remaps, - log_cmd=True, - ), - ], - deprecated_reason="Deprecated in favor of camera.launch.py" - ) diff --git a/launch/examples/example_o3r_2d_and_3d.launch.py b/launch/examples/example_o3r_2d_and_3d.launch.py index 4a100ca..5f753ba 100644 --- a/launch/examples/example_o3r_2d_and_3d.launch.py +++ b/launch/examples/example_o3r_2d_and_3d.launch.py @@ -1,6 +1,6 @@ # # SPDX-License-Identifier: Apache-2.0 -# Copyright (C) 2019 ifm electronic, gmbh +# Copyright (C) 2024 ifm electronic, gmbh # from launch import LaunchDescription diff --git a/launch/examples/example_two_o3r_heads.launch.py b/launch/examples/example_two_o3r_heads.launch.py index 8204434..85ce516 100644 --- a/launch/examples/example_two_o3r_heads.launch.py +++ b/launch/examples/example_two_o3r_heads.launch.py @@ -1,6 +1,6 @@ # # SPDX-License-Identifier: Apache-2.0 -# Copyright (C) 2019 ifm electronic, gmbh +# Copyright (C) 2024 ifm electronic, gmbh # from launch import LaunchDescription diff --git a/launch/multi_cameras.launch.py b/launch/multi_cameras.launch.py deleted file mode 100644 index 7e9d2f8..0000000 --- a/launch/multi_cameras.launch.py +++ /dev/null @@ -1,80 +0,0 @@ -#!/usr/bin/env python3 - -# -# SPDX-License-Identifier: Apache-2.0 -# Copyright (C) 2019 ifm electronic, gmbh -# - -import yaml -import logging - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import LogInfo -from launch.actions import IncludeLaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch.actions import OpaqueFunction - -deprecation_warning = LogInfo( - msg=""" - - ###################################################################################### - # # - # This launch script is deprecated. Use the parametrized camera.launch.py instead! # - # # - ###################################################################################### - """ -) - -ifm3d_ros2_dir = get_package_share_directory('ifm3d_ros2') - -logging.basicConfig(format='%(asctime)s - %(message)s', level=logging.INFO) - -def add_camera_loop(context, *args, **kwargs): - - cameras=[] - params_path = LaunchConfiguration('params').perform(context) - namespace = LaunchConfiguration('namespace').perform(context) - logging.debug(vars(context)) - - with open(params_path) as p: - params_data = yaml.load(p, Loader=yaml.FullLoader) - - try: - print(params_data.keys()) - for key, value in params_data.items(): - logging.info('Camera port arguments: {}'.format(key.split(sep='/')[-1])) - - camera = IncludeLaunchDescription( - PythonLaunchDescriptionSource(ifm3d_ros2_dir + '/launch/camera_managed.launch.py'), - launch_arguments={"name": key.split(sep='{}/'.format(namespace))[-1], - "params": params_path, - "namespace": namespace, - }.items(), - ) - logging.info('Launch argument: {}'.format(vars(camera))) - - # add camera to cameras list as parsed from namespace arg and yaml fields - cameras.append(camera) - except Exception as e: - print('parsing the configuration yaml found an error: {}. Please check your namespace definitions.'.format(e)) - return cameras - - -def generate_launch_description(): - """Launch the camera_managed.launch.py launch file.""" - ld = LaunchDescription(deprecated_reason="Deprecated in favor of camera.launch.py") - ld.add_action(deprecation_warning) - DeclareLaunchArgument( - 'namespace', - default_value='ifm3d', - ) - DeclareLaunchArgument( - 'params', - default_value = ifm3d_ros2_dir + '/config/params.yaml', - description='test arg that overlaps arg in included file', - ) - ld.add_action(OpaqueFunction(function = add_camera_loop)) - return ld \ No newline at end of file diff --git a/launch/ods.launch.py b/launch/ods.launch.py new file mode 100644 index 0000000..4e9cef3 --- /dev/null +++ b/launch/ods.launch.py @@ -0,0 +1,148 @@ +# +# SPDX-License-Identifier: Apache-2.0 +# Copyright (C) 2024 ifm electronic, gmbh +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler +from launch.conditions import IfCondition +from launch.events import matches_action +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import LifecycleNode, Node +from launch_ros.events.lifecycle import ChangeState +from launch_ros.event_handlers import OnStateTransition +from launch_ros.substitutions import FindPackageShare +from lifecycle_msgs.msg import Transition + + +def generate_launch_description(): + + # ------------------------------------------------------------ + # Launch script arguments + # ------------------------------------------------------------ + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "log_level", + default_value="info", + description="To change RCLCPP log level for the camera node. ['debug', 'info', 'warn', 'error']", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "visualization", + default_value="false", + description="If true, RViz2 with a predefined config is opened.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ods_name", + default_value="ods", + description="Name for the ODS node and used as tf prefix", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ods_namespace", + default_value="ifm3d", + description="Namespace to launch nodes into", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "parameter_file_package", + default_value="ifm3d_ros2", + description="Package containing the camera's YAML configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "parameter_file_directory", + default_value="config", + description="Directory inside of parameter_file_package containing the camera's YAML configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "parameter_file_name", + default_value="ods_default_parameters.yaml", + description="YAML file with the camera configuration.", + ) + ) + + # ------------------------------------------------------------ + # Nodes, using substitutions to fill in arguments + # ------------------------------------------------------------ + ods_node = LifecycleNode( + package="ifm3d_ros2", + executable="ods_standalone", + namespace=LaunchConfiguration("ods_namespace"), + name=LaunchConfiguration("ods_name"), + output='screen', + parameters=[ + PathJoinSubstitution( + [ + FindPackageShare(LaunchConfiguration("parameter_file_package")), + LaunchConfiguration("parameter_file_directory"), + LaunchConfiguration("parameter_file_name"), + ] + ) + ], + arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], + log_cmd=True, + ) + + # Launching RViz2 conditionally, depending on the "visualition" argument + rviz_node = Node( + executable="rviz2", + package="rviz2", + name=[LaunchConfiguration("ods_name"), "_rviz2"], + arguments=[ + '-d', + PathJoinSubstitution([FindPackageShare("ifm3d_ros2"), "etc", "ifm3d.rviz"]), + ], + condition=IfCondition(LaunchConfiguration("visualization")), + log_cmd=True, + ) + + # ------------------------------------------------------------ + # Lifecycle management + # ------------------------------------------------------------ + + # UNCONFIGURED to INACTIVE via ChangeState (emitted without delay) + configure_ods = EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(ods_node), + transition_id=Transition.TRANSITION_CONFIGURE, + ) + ) + + # INACTIVE to ACTIVE via Handler + # Handler emits event after the transition from configuring to inactive + # Emitted ChangeState event transitions the node from inactive to active + activate_ods = RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=ods_node, + start_state='configuring', + goal_state='inactive', + entities=[ + EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(ods_node), + transition_id=Transition.TRANSITION_ACTIVATE, + ) + ) + ], + ) + ) + + ld = LaunchDescription() + for declared_argument in declared_arguments: + ld.add_entity(declared_argument) + ld.add_action(ods_node) + ld.add_action(rviz_node) + ld.add_action(configure_ods) + ld.add_action(activate_ods) + + return ld diff --git a/launch/rviz.launch.py b/launch/rviz.launch.py deleted file mode 100644 index 60df3e6..0000000 --- a/launch/rviz.launch.py +++ /dev/null @@ -1,41 +0,0 @@ -# -# SPDX-License-Identifier: Apache-2.0 -# Copyright (C) 2019 ifm electronic, gmbh -# - -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import ExecuteProcess, LogInfo - -deprecation_warning = LogInfo( - msg=""" - - ###################################################################################### - # # - # This launch script is deprecated. Use the parametrized camera.launch.py instead! # - # # - ###################################################################################### - """ -) - - -def generate_launch_description(): - package_name = 'ifm3d_ros2' - - rviz_config = os.path.join( - get_package_share_directory(package_name), 'etc', 'ifm3d.rviz' - ) - - return LaunchDescription( - [ - deprecation_warning, - ExecuteProcess( - cmd=['ros2', 'run', 'rviz2', 'rviz2', '-d', rviz_config], - output='screen', - log_cmd=True, - ), - ], - deprecated_reason="Deprecated in favor of camera.launch.py" - ) diff --git a/msg/Zones.msg b/msg/Zones.msg new file mode 100644 index 0000000..5776d33 --- /dev/null +++ b/msg/Zones.msg @@ -0,0 +1,4 @@ +std_msgs/Header header + +uint8[3] zone_occupied +uint32 zone_config_id \ No newline at end of file diff --git a/package.xml b/package.xml index fb23d4d..1adacaf 100644 --- a/package.xml +++ b/package.xml @@ -12,41 +12,28 @@ ament_cmake_auto ament_cmake_ros - builtin_interfaces - diagnostic_msgs + builtin_interfaces + diagnostic_msgs + launch + launch_ros + lifecycle_msgs + nav_msgs + nav2_msgs + rclcpp + rclcpp_components + rclcpp_lifecycle + rcl_interfaces + rclpy + rmw + ros2launch + rosidl_default_generators + sensor_msgs + std_msgs + tf2_ros + geometry_msgs - launch - launch_ros - lifecycle_msgs - rclcpp - rclcpp_components - rclcpp_lifecycle - rcl_interfaces - rclpy - rmw - ros2launch - rosidl_default_generators - sensor_msgs - std_msgs - tf2_ros ament_index_python - builtin_interfaces - diagnostic_msgs - launch - launch_ros - lifecycle_msgs - rclcpp - rclcpp_components - rclcpp_lifecycle - rcl_interfaces - rclpy - rmw - ros2launch - rosidl_default_runtime - sensor_msgs - std_msgs - tf2_ros launch_testing_ament_cmake python3-opencv diff --git a/scripts/config b/scripts/config deleted file mode 100644 index 1c622b3..0000000 --- a/scripts/config +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python3 -# -*- python -*- - -# -# SPDX-License-Identifier: Apache-2.0 -# Copyright (C) 2019 ifm electronic, gmbh -# - -import argparse -import json -import sys - -import rclpy -from ifm3d_ros2.srv import Config - -WAIT_SECS = 2.0 -SRV_NAME = "/ifm3d/camera/Config" -NODE_NAME = "ifm3d_ros2_config_client" - -def get_args(): - parser = argparse.ArgumentParser( - description='Configure an ifm3d camera from JSON piped to stdin', - formatter_class=argparse.ArgumentDefaultsHelpFormatter) - - parser.add_argument('--srv', required=False, default=SRV_NAME, - help="The fully qualified `Config' service to call") - parser.add_argument('--node', required=False, default=NODE_NAME, - help="The node name of this service client") - - args = parser.parse_args(sys.argv[1:]) - return args - -def main(): - args = get_args() - - rclpy.init() - node = rclpy.create_node(args.node) - cli = node.create_client(Config, args.srv) - req = Config.Request() - log = node.get_logger() - - try: - req.json = json.dumps(json.load(sys.stdin)) - - while not cli.wait_for_service(timeout_sec=WAIT_SECS): - log.info("Config service not available, waiting...") - - fut = cli.call_async(req) - rclpy.spin_until_future_complete(node, fut) - - res = fut.result() - if res is not None: - if res.status != 0: - log.error("Config failed with error: %s - %s" % - (str(res.status), res.msg)) - else: - log.warn("Service call failed %r" % (fut.exception(),)) - - except KeyboardInterrupt: - pass - - finally: - node.destroy_node() - rclpy.shutdown() - - return 0 - -if __name__ == '__main__': - sys.exit(main()) diff --git a/scripts/dump b/scripts/dump deleted file mode 100644 index 8c41893..0000000 --- a/scripts/dump +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/env python3 -# -*- python -*- - -# -# SPDX-License-Identifier: Apache-2.0 -# Copyright (C) 2019 ifm electronic, gmbh -# - -import argparse -import json -import sys - -import rclpy -from ifm3d_ros2.srv import Dump - -WAIT_SECS = 2.0 -SRV_NAME = "/ifm3d/camera/Dump" -NODE_NAME = "ifm3d_ros2_dump_client" - -def get_args(): - parser = argparse.ArgumentParser( - description='Dump an ifm3d camera configuration to stdout as JSON', - formatter_class=argparse.ArgumentDefaultsHelpFormatter) - - parser.add_argument('--srv', required=False, default=SRV_NAME, - help="The fully qualified `Dump' service to call") - parser.add_argument('--node', required=False, default=NODE_NAME, - help="The node name of this service client") - - args = parser.parse_args(sys.argv[1:]) - return args - -def main(): - args = get_args() - - rclpy.init() - node = rclpy.create_node(args.node) - cli = node.create_client(Dump, args.srv) - req = Dump.Request() - log = node.get_logger() - - try: - while not cli.wait_for_service(timeout_sec=WAIT_SECS): - log.info("Dump service not available, waiting...") - - fut = cli.call_async(req) - rclpy.spin_until_future_complete(node, fut) - - res = fut.result() - if res is not None: - if res.status == 0: - print(json.dumps(json.loads(res.config), - sort_keys=True, indent=4, separators=(',', ': '))) - else: - log.error("Dump failed with error: %s - %s" % - (str(res.status), - "Check the `ifm3d' logs for more detail")) - else: - log.warn("Service call failed %r" % (fut.exception(),)) - - except KeyboardInterrupt: - pass - - finally: - node.destroy_node() - rclpy.shutdown() - - return 0 - -if __name__ == '__main__': - sys.exit(main()) diff --git a/scripts/launch_in_docker.sh b/scripts/launch_in_docker.sh deleted file mode 100755 index e4b6e86..0000000 --- a/scripts/launch_in_docker.sh +++ /dev/null @@ -1,24 +0,0 @@ -#!/bin/sh - -print_help() -{ - echo Convenience script for launching a ifm3d-ros2 launchfile from a docker container. - echo - echo \$1: Docker image name - echo "\$2: Launchfile (Optional; Default: 'camera_managed.launch.py')" - exit 22 -} - -test $# -lt 1 && print_help - -image=${1} -shift -launchfile=${1:-camera_managed.launch.py} -test $# -ge 1 && shift - -# Including "-it" so that CTRL-C works -docker run -it -p 11311:11311 $image \ - sh -c ". /opt/ros/foxy/setup.sh; \ - . /home/ifm/colcon_ws/ifm3d-ros2/install/setup.sh; \ - ros2 launch ifm3d_ros2 $launchfile $@" - diff --git a/src/bin/camera_standalone.cpp b/src/bin/camera_standalone.cpp index 1278176..8fe6f20 100644 --- a/src/bin/camera_standalone.cpp +++ b/src/bin/camera_standalone.cpp @@ -1,6 +1,6 @@ /* * SPDX-License-Identifier: Apache-2.0 - * Copyright (C) 2019 ifm electronic, gmbh + * Copyright (C) 2024 ifm electronic, gmbh */ #include diff --git a/src/bin/ods_standalone.cpp b/src/bin/ods_standalone.cpp new file mode 100644 index 0000000..b1f6440 --- /dev/null +++ b/src/bin/ods_standalone.cpp @@ -0,0 +1,23 @@ +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#include +#include +#include +#include + +int main(int argc, char** argv) +{ + std::setvbuf(stdout, NULL, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor exec; + rclcpp::NodeOptions options; + auto ods_node = std::make_shared(options); + exec.add_node(ods_node->get_node_base_interface()); + exec.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/src/lib/buffer_conversions.cpp b/src/lib/buffer_conversions.cpp new file mode 100644 index 0000000..72ef4f9 --- /dev/null +++ b/src/lib/buffer_conversions.cpp @@ -0,0 +1,788 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ifm3d_ros2 +{ +sensor_msgs::msg::Image ifm3d_to_ros_image(ifm3d::Buffer& image, // Need non-const image because image.begin(), + // image.end() don't have const overloads. + const std_msgs::msg::Header& header, const rclcpp::Logger& logger) +{ + static constexpr auto max_pixel_format = static_cast(ifm3d::pixel_format::FORMAT_32F3); + static constexpr auto image_format_info = [] { + auto image_format_info = std::array{}; + + { + using namespace ifm3d; + using namespace sensor_msgs::image_encodings; + image_format_info[static_cast(pixel_format::FORMAT_8U)] = TYPE_8UC1; + image_format_info[static_cast(pixel_format::FORMAT_8S)] = TYPE_8SC1; + image_format_info[static_cast(pixel_format::FORMAT_16U)] = TYPE_16UC1; + image_format_info[static_cast(pixel_format::FORMAT_16S)] = TYPE_16SC1; + image_format_info[static_cast(pixel_format::FORMAT_32U)] = "32UC1"; + image_format_info[static_cast(pixel_format::FORMAT_32S)] = TYPE_32SC1; + image_format_info[static_cast(pixel_format::FORMAT_32F)] = TYPE_32FC1; + image_format_info[static_cast(pixel_format::FORMAT_64U)] = "64UC1"; + image_format_info[static_cast(pixel_format::FORMAT_64F)] = TYPE_64FC1; + image_format_info[static_cast(pixel_format::FORMAT_16U2)] = TYPE_16UC2; + image_format_info[static_cast(pixel_format::FORMAT_32F3)] = TYPE_32FC3; + } + + return image_format_info; + }(); + + const auto format = static_cast(image.dataFormat()); + + sensor_msgs::msg::Image result{}; + result.header = header; + result.height = image.height(); + result.width = image.width(); + result.is_bigendian = 0; + + if (image.begin() == image.end()) + { + return result; + } + + if (format >= max_pixel_format) + { + RCLCPP_ERROR(logger, "Pixel format out of range (%ld >= %ld)", format, max_pixel_format); + return result; + } + + result.encoding = image_format_info.at(format); + result.step = result.width * sensor_msgs::image_encodings::bitDepth(image_format_info.at(format)) / 8; + result.data.insert(result.data.end(), image.ptr<>(0), std::next(image.ptr<>(0), result.step * result.height)); + + if (result.encoding.empty()) + { + RCLCPP_WARN(logger, "Can't handle encoding %ld (32U == %ld, 64U == %ld)", format, + static_cast(ifm3d::pixel_format::FORMAT_32U), + static_cast(ifm3d::pixel_format::FORMAT_64U)); + result.encoding = sensor_msgs::image_encodings::TYPE_8UC1; + } + + return result; +} + +sensor_msgs::msg::Image ifm3d_to_ros_image(ifm3d::Buffer&& image, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + return ifm3d_to_ros_image(image, header, logger); +} + +sensor_msgs::msg::CompressedImage ifm3d_to_ros_compressed_image(ifm3d::Buffer& image, // Need non-const image because + // image.begin(), image.end() + // don't have const overloads. + const std_msgs::msg::Header& header, + const std::string& format, // "jpeg" or "png" + const rclcpp::Logger& logger) +{ + sensor_msgs::msg::CompressedImage result{}; + result.header = header; + result.format = format; + + if (const auto dataFormat = image.dataFormat(); + dataFormat != ifm3d::pixel_format::FORMAT_8S && dataFormat != ifm3d::pixel_format::FORMAT_8U) + { + RCLCPP_ERROR(logger, "Invalid data format for %s data (%ld)", format.c_str(), static_cast(dataFormat)); + return result; + } + + result.data.insert(result.data.end(), image.ptr<>(0), std::next(image.ptr<>(0), image.width() * image.height())); + return result; +} + +sensor_msgs::msg::CompressedImage ifm3d_to_ros_compressed_image(ifm3d::Buffer&& image, + const std_msgs::msg::Header& header, + const std::string& format, const rclcpp::Logger& logger) +{ + return ifm3d_to_ros_compressed_image(image, header, format, logger); +} + +sensor_msgs::msg::PointCloud2 ifm3d_to_ros_cloud(ifm3d::Buffer& image, // Need non-const image because image.begin(), + // image.end() don't have const overloads. + const std_msgs::msg::Header& header, const rclcpp::Logger& logger) +{ + sensor_msgs::msg::PointCloud2 result{}; + result.header = header; + result.height = image.height(); + result.width = image.width(); + result.is_bigendian = false; + + if (image.begin() == image.end()) + { + return result; + } + + if (image.dataFormat() != ifm3d::pixel_format::FORMAT_32F3 && image.dataFormat() != ifm3d::pixel_format::FORMAT_32F) + { + RCLCPP_ERROR(logger, "Unsupported pixel format %ld for point cloud", static_cast(image.dataFormat())); + return result; + } + + sensor_msgs::msg::PointField x_field{}; + x_field.name = "x"; + x_field.offset = 0; + x_field.datatype = sensor_msgs::msg::PointField::FLOAT32; + x_field.count = 1; + + sensor_msgs::msg::PointField y_field{}; + y_field.name = "y"; + y_field.offset = 4; + y_field.datatype = sensor_msgs::msg::PointField::FLOAT32; + y_field.count = 1; + + sensor_msgs::msg::PointField z_field{}; + z_field.name = "z"; + z_field.offset = 8; + z_field.datatype = sensor_msgs::msg::PointField::FLOAT32; + z_field.count = 1; + + result.fields = { + x_field, + y_field, + z_field, + }; + + result.point_step = result.fields.size() * sizeof(float); + result.row_step = result.point_step * result.width; + result.is_dense = true; + result.data.insert(result.data.end(), image.ptr<>(0), std::next(image.ptr<>(0), result.row_step * result.height)); + + return result; +} + +sensor_msgs::msg::PointCloud2 ifm3d_to_ros_cloud(ifm3d::Buffer&& image, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + return ifm3d_to_ros_cloud(image, header, logger); +} + +nav_msgs::msg::MapMetaData ifm3d_to_ros_map_meta_data(ifm3d::ODSOccupancyGridV1 grid, const rclcpp::Logger& logger) +{ + // values of matrix 2x3 affine mapping between grid cell and user coordinate system + // e.g, multiplying the matrix with [0,0,1] gives the user coordinate of the center of upper left cell + const std::array mapping_matrix = grid.transform_cell_center_to_user; + RCLCPP_ERROR_EXPRESSION(logger, mapping_matrix[0] != mapping_matrix[4], + "Cells of the received ODSOccupancyGrid are not quadratic!"); + + nav_msgs::msg::MapMetaData map_meta_data; + map_meta_data.map_load_time = rclcpp::Time(grid.timestamp_ns); + map_meta_data.resolution = mapping_matrix[0]; + map_meta_data.width = grid.width; + map_meta_data.height = grid.height; + + // The origin of the costmap [m, m, rad]. + // This is the real-world pose of the cell (0,0) in the map. + map_meta_data.origin.position.x = mapping_matrix[2]; + map_meta_data.origin.position.y = mapping_matrix[5]; + map_meta_data.origin.position.z = 0.0; + map_meta_data.origin.orientation.x = 0; // we assume no rotation is present + map_meta_data.origin.orientation.y = 0; + map_meta_data.origin.orientation.z = 0; + map_meta_data.origin.orientation.w = 1; + + return map_meta_data; +} + +nav_msgs::msg::OccupancyGrid ifm3d_to_ros_occupancy_grid(ifm3d::Buffer& image, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + RCLCPP_DEBUG(logger, "Deserializing occupancy grid data"); + auto occupancy_grid_data = ifm3d::ODSOccupancyGridV1::Deserialize(image); + nav_msgs::msg::OccupancyGrid occupancy_grid_msg; + occupancy_grid_msg.header = header; + occupancy_grid_msg.header.stamp = rclcpp::Time(occupancy_grid_data.timestamp_ns); + occupancy_grid_msg.info = ifm3d_to_ros_map_meta_data(occupancy_grid_data, logger); + // Allocate a single contiguous block of memory for the 2D array + std::vector data(occupancy_grid_data.width * occupancy_grid_data.height); + + for (u_int32_t i = 0; i < occupancy_grid_data.width; i++) + { + for (u_int32_t j = 0; j < occupancy_grid_data.height; j++) + { + // Get the value from the image + uint8_t value = occupancy_grid_data.image.at(i, j); + + // Scale the value to the range 0-100 + int8_t scaled_value = static_cast((value * 100) / 255); + + // Assume values at or below 20 to be free, set their value to zero + scaled_value = (scaled_value <= 20) ? 0 : scaled_value; + + data[i * occupancy_grid_data.height + j] = scaled_value; + } + } + occupancy_grid_msg.data = data; + return occupancy_grid_msg; +} + +nav_msgs::msg::OccupancyGrid ifm3d_to_ros_occupancy_grid(ifm3d::Buffer&& image, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + return ifm3d_to_ros_occupancy_grid(image, header, logger); +} + +nav2_msgs::msg::Costmap ifm3d_to_ros_costmap(ifm3d::Buffer& image, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + RCLCPP_DEBUG(logger, "Generating costmap..."); + auto occupancy_grid_data = ifm3d::ODSOccupancyGridV1::Deserialize(image); + nav2_msgs::msg::Costmap costmap_msg; + costmap_msg.header = header; + costmap_msg.header.stamp = rclcpp::Time(occupancy_grid_data.timestamp_ns); + RCLCPP_ERROR_EXPRESSION(logger, costmap_msg.header.frame_id.empty(), + "frame_id is not set in the header provided to ifm3d_to_ros_costmap()!"); + + costmap_msg.metadata.update_time = costmap_msg.header.stamp; + costmap_msg.metadata.layer = "ods"; + + // Use MapMetaData to populate Costmap metadata + const nav_msgs::msg::MapMetaData meta_data = ifm3d_to_ros_map_meta_data(occupancy_grid_data, logger); + costmap_msg.metadata.resolution = meta_data.resolution; + costmap_msg.metadata.size_x = meta_data.width; + costmap_msg.metadata.size_y = meta_data.height; + costmap_msg.metadata.origin = meta_data.origin; + + // Allocate a single contiguous block of memory for the 2D array + std::vector data(occupancy_grid_data.width * occupancy_grid_data.height); + + for (uint32_t i = 0; i < occupancy_grid_data.width; i++) + { + for (uint32_t j = 0; j < occupancy_grid_data.height; j++) + { + // Get the value from the image + uint8_t value = occupancy_grid_data.image.at(i, j); + // Scale the value to the range 0-100 + int8_t scaled_value = static_cast((value * 100) / 255); + // data[i * occupancy_grid_data.height + j] = occupancy_grid_data.image.at(i, occupancy_grid_data.height + // - 1 - j); + data[i * occupancy_grid_data.height + j] = scaled_value; + } + } + costmap_msg.data = data; + + RCLCPP_DEBUG(logger, "Costmap generated."); + return costmap_msg; +} + +nav2_msgs::msg::Costmap ifm3d_to_ros_costmap(ifm3d::Buffer&& image, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + return ifm3d_to_ros_costmap(image, header, logger); +} + +ifm3d_ros2::msg::Extrinsics ifm3d_to_extrinsics(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + ifm3d_ros2::msg::Extrinsics extrinsics_msg; + extrinsics_msg.header = header; + try + { + extrinsics_msg.tx = buffer.at(0); + extrinsics_msg.ty = buffer.at(1); + extrinsics_msg.tz = buffer.at(2); + extrinsics_msg.rot_x = buffer.at(3); + extrinsics_msg.rot_y = buffer.at(4); + extrinsics_msg.rot_z = buffer.at(5); + } + catch (const std::out_of_range& ex) + { + RCLCPP_WARN(logger, "Out-of-range error fetching extrinsics"); + } + return extrinsics_msg; +} + +ifm3d_ros2::msg::Extrinsics ifm3d_to_extrinsics(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + return ifm3d_to_extrinsics(buffer, header, logger); +} + +bool ifm3d_rgb_info_to_camera_info(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, const uint32_t height, + const uint32_t width, const rclcpp::Logger& logger, + sensor_msgs::msg::CameraInfo& camera_info_msg) +{ + camera_info_msg.header = header; + + try + { + auto rgb_info = ifm3d::RGBInfoV1::Deserialize(buffer); + camera_info_msg = ifm3d_to_camera_info(rgb_info.intrinsic_calibration, header, height, width, logger); + return true; + } + catch (...) + { + RCLCPP_ERROR(logger, "Failed to read rgb info."); + return false; + } +} + +bool ifm3d_rgb_info_to_camera_info(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, const uint32_t height, + const uint32_t width, const rclcpp::Logger& logger, + sensor_msgs::msg::CameraInfo& camera_info_msg) +{ + return ifm3d_rgb_info_to_camera_info(buffer, header, height, width, logger, camera_info_msg); +} + +bool ifm3d_tof_info_to_camera_info(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, const uint32_t height, + const uint32_t width, const rclcpp::Logger& logger, + sensor_msgs::msg::CameraInfo& camera_info_msg) +{ + camera_info_msg.header = header; + + try + { + auto tof_info = ifm3d::TOFInfoV4::Deserialize(buffer); + camera_info_msg = ifm3d_to_camera_info(tof_info.intrinsic_calibration, header, height, width, logger); + return true; + } + catch (...) + { + RCLCPP_ERROR(logger, "Failed to read rgb info."); + return false; + } +} + +bool ifm3d_tof_info_to_camera_info(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, const uint32_t height, + const uint32_t width, const rclcpp::Logger& logger, + sensor_msgs::msg::CameraInfo& camera_info_msg) +{ + return ifm3d_tof_info_to_camera_info(buffer, header, height, width, logger, camera_info_msg); +} + +sensor_msgs::msg::CameraInfo ifm3d_to_camera_info(ifm3d::calibration::IntrinsicCalibration intrinsic, + const std_msgs::msg::Header& header, const uint32_t height, + const uint32_t width, const rclcpp::Logger& logger) +{ + sensor_msgs::msg::CameraInfo camera_info_msg; + camera_info_msg.header = header; + + try + { + camera_info_msg.height = height; + camera_info_msg.width = width; + if (intrinsic.model_id == 2) // This is a fish eye lens + { + // Fill in the message with fish eye model params + camera_info_msg.distortion_model = sensor_msgs::distortion_models::EQUIDISTANT; + + // Read data from buffer + const float fx = intrinsic.model_parameters[0]; + const float fy = intrinsic.model_parameters[1]; + + const float mx = intrinsic.model_parameters[2]; + const float my = intrinsic.model_parameters[3]; + const float alpha = intrinsic.model_parameters[4]; + const float k1 = intrinsic.model_parameters[5]; + const float k2 = intrinsic.model_parameters[6]; + const float k3 = intrinsic.model_parameters[7]; + const float k4 = intrinsic.model_parameters[8]; + const float theta_max = intrinsic.model_parameters[9]; + + const float ix = width - 1; + const float iy = height - 1; + const float cy = (iy + 0.5 - my) / fy; + const float cx = (ix + 0.5 - mx) / fx - alpha * cy; + const float r2 = cx * cx + cy * cy; + const float h = 2 * cx * cy; + const float tx = k3 * h + k4 * (r2 + 2 * cx * cx); + const float ty = k3 * (r2 + 2 * cy * cy) + k4 * h; + + // Distortion parameters + camera_info_msg.d.resize(5); + camera_info_msg.d[0] = k1; + camera_info_msg.d[1] = k2; + camera_info_msg.d[2] = tx; // TODO t1 == tx ? + camera_info_msg.d[3] = ty; // TODO t2 == ty ? + camera_info_msg.d[4] = k3; + + // Intrinsic camera matrix, in row-major order + // [ fx 0 cx] + // K = [ 0 fy cy] + // [ 0 0 1 ] + camera_info_msg.k[0] = fx; + camera_info_msg.k[2] = cx; + camera_info_msg.k[4] = fy; + camera_info_msg.k[5] = cy; + camera_info_msg.k[8] = 1.0; // fixed to 1.0 + + // Projection matrix, row-major + // [fx' 0 cx' Tx] + // P = [ 0 fy' cy' Ty] + // [ 0 0 1 0 ] + camera_info_msg.p[0] = fx; + camera_info_msg.p[5] = fy; + camera_info_msg.p[2] = cx; + camera_info_msg.p[6] = cy; + camera_info_msg.p[10] = 1.0; // fixed to 1.0 + + RCLCPP_DEBUG_ONCE(logger, + "Intrinsics:\nfx=%f \nfy=%f \nmx=%f \nmy=%f \nalpha=%f \nk1=%f \nk2=%f \nk3=%f \nk4=%f " + "\nCalculated:\nix=%f \niy=%f \ncx=%f \ncy=%f \nr2=%f \nh=%f \ntx=%f \nty=%f", + fx, fy, mx, my, alpha, k1, k2, k3, k4, ix, iy, cx, cy, r2, h, tx, ty); + } + else if (intrinsic.model_id == 0) + { + camera_info_msg.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + + // Read data from buffer + const float fx = intrinsic.model_parameters[0]; + const float fy = intrinsic.model_parameters[1]; + + const float mx = intrinsic.model_parameters[2]; + const float my = intrinsic.model_parameters[3]; + const float alpha = intrinsic.model_parameters[4]; + const float k1 = intrinsic.model_parameters[5]; + const float k2 = intrinsic.model_parameters[6]; + const float k3 = intrinsic.model_parameters[7]; + const float k4 = intrinsic.model_parameters[8]; + const float k5 = intrinsic.model_parameters[9]; + + const float ix = width - 1; + const float iy = height - 1; + const float cy = (iy + 0.5 - my) / fy; + const float cx = (ix + 0.5 - mx) / fx - alpha * cy; + const float r2 = cx * cx + cy * cy; + const float h = 2 * cx * cy; + const float tx = k3 * h + k4 * (r2 + 2 * cx * cx); + const float ty = k3 * (r2 + 2 * cy * cy) + k4 * h; + + // Distortion parameters + camera_info_msg.d.resize(5); + camera_info_msg.d[0] = k1; + camera_info_msg.d[1] = k2; + camera_info_msg.d[2] = tx; // TODO t1 == tx ? + camera_info_msg.d[3] = ty; // TODO t2 == ty ? + camera_info_msg.d[4] = k3; + + // Intrinsic camera matrix + camera_info_msg.k[0] = fx; + camera_info_msg.k[4] = fy; + camera_info_msg.k[2] = cx; + camera_info_msg.k[5] = cy; + camera_info_msg.k[8] = 1.0; // fixed to 1.0 + + // Projection matrix + camera_info_msg.p[0] = fx; + camera_info_msg.p[5] = fy; + camera_info_msg.p[2] = cx; + camera_info_msg.p[6] = cy; + camera_info_msg.p[10] = 1.0; // fixed to 1.0 + + RCLCPP_DEBUG_ONCE(logger, + "Intrinsics:\nfx=%f \nfy=%f \nmx=%f \nmy=%f \nalpha=%f \nk1=%f \nk2=%f \nk3=%f \nk4=%f " + "\nCalculated:\nix=%f \niy=%f \ncx=%f \ncy=%f \nr2=%f \nh=%f \ntx=%f \nty=%f", + fx, fy, mx, my, alpha, k1, k2, k3, k4, ix, iy, cx, cy, r2, h, tx, ty); + } + else{ + RCLCPP_ERROR(logger, "Unknown intrinsic calibration model"); + } + } + catch (const std::out_of_range& ex) + { + RCLCPP_WARN(logger, "Out-of-range error fetching intrinsics"); + } + return camera_info_msg; +} + +ifm3d_ros2::msg::Intrinsics ifm3d_to_intrinsics(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + ifm3d_ros2::msg::Intrinsics intrinsics_msg; + intrinsics_msg.header = header; + + ifm3d::calibration::IntrinsicCalibration intrinsics; + + try + { + intrinsics.Read(buffer.ptr(0)); + intrinsics_msg.model_id = intrinsics.model_id; + intrinsics_msg.model_parameters = intrinsics.model_parameters; + } + catch (...) + { + RCLCPP_ERROR(logger, "Failed to read intrinsics."); + } + + return intrinsics_msg; +} + +ifm3d_ros2::msg::Intrinsics ifm3d_to_intrinsics(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) + +{ + return ifm3d_to_intrinsics(buffer, header, logger); +} + +ifm3d_ros2::msg::InverseIntrinsics ifm3d_to_inverse_intrinsics(ifm3d::Buffer& buffer, + const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + ifm3d_ros2::msg::InverseIntrinsics inverse_intrinsics_msg; + inverse_intrinsics_msg.header = header; + + ifm3d::calibration::InverseIntrinsicCalibration inverse_intrinsics; + + try + { + inverse_intrinsics.Read(buffer.ptr(0)); + inverse_intrinsics_msg.model_id = inverse_intrinsics.model_id; + inverse_intrinsics_msg.model_parameters = inverse_intrinsics.model_parameters; + } + catch (...) + { + RCLCPP_ERROR(logger, "Failed to read inverse intrinsics."); + } + + return inverse_intrinsics_msg; +} + +ifm3d_ros2::msg::InverseIntrinsics ifm3d_to_inverse_intrinsics(ifm3d::Buffer&& buffer, + const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) + +{ + return ifm3d_to_inverse_intrinsics(buffer, header, logger); +} + +ifm3d_ros2::msg::RGBInfo ifm3d_to_rgb_info(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + ifm3d_ros2::msg::RGBInfo rgb_info_msg; + rgb_info_msg.header = header; + + try + { + auto rgb_info = ifm3d::RGBInfoV1::Deserialize(buffer); + + rgb_info_msg.version = rgb_info.version; + rgb_info_msg.frame_counter = rgb_info.frame_counter; + rgb_info_msg.timestamp_ns = rgb_info.timestamp_ns; + rgb_info_msg.exposure_time = rgb_info.exposure_time; + + rgb_info_msg.extrinsics.header = header; + rgb_info_msg.extrinsics.tx = rgb_info.extrinsic_optic_to_user.trans_x; + rgb_info_msg.extrinsics.ty = rgb_info.extrinsic_optic_to_user.trans_y; + rgb_info_msg.extrinsics.tz = rgb_info.extrinsic_optic_to_user.trans_z; + rgb_info_msg.extrinsics.rot_x = rgb_info.extrinsic_optic_to_user.rot_x; + rgb_info_msg.extrinsics.rot_y = rgb_info.extrinsic_optic_to_user.rot_y; + rgb_info_msg.extrinsics.rot_z = rgb_info.extrinsic_optic_to_user.rot_z; + + rgb_info_msg.intrinsics.header = header; + rgb_info_msg.intrinsics.model_id = rgb_info.intrinsic_calibration.model_id; + rgb_info_msg.intrinsics.model_parameters = rgb_info.intrinsic_calibration.model_parameters; + + rgb_info_msg.inverse_intrinsics.header = header; + rgb_info_msg.inverse_intrinsics.model_id = rgb_info.inverse_intrinsic_calibration.model_id; + rgb_info_msg.inverse_intrinsics.model_parameters = rgb_info.inverse_intrinsic_calibration.model_parameters; + } + catch (...) + { + RCLCPP_ERROR(logger, "Failed to read rgb info."); + } + + return rgb_info_msg; +} + +ifm3d_ros2::msg::RGBInfo ifm3d_to_rgb_info(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) + +{ + return ifm3d_to_rgb_info(buffer, header, logger); +} + +ifm3d_ros2::msg::TOFInfo ifm3d_to_tof_info(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + ifm3d_ros2::msg::TOFInfo tof_info_msg; + tof_info_msg.header = header; + + try + { + auto tof_info = ifm3d::TOFInfoV4::Deserialize(buffer); + tof_info_msg.measurement_block_index = tof_info.measurement_block_index; + tof_info_msg.measurement_range_min = tof_info.measurement_range_min; + tof_info_msg.measurement_range_max = tof_info.measurement_range_max; + tof_info_msg.version = tof_info.version; + tof_info_msg.distance_resolution = tof_info.distance_resolution; + tof_info_msg.amplitude_resolution = tof_info.amplitude_resolution; + tof_info_msg.amp_normalization_factors = tof_info.amp_normalization_factors; + tof_info_msg.exposure_timestamps_ns = tof_info.exposure_timestamps_ns; + tof_info_msg.exposure_times_s = tof_info.exposure_times_s; + tof_info_msg.illu_temperature = tof_info.illu_temperature; + tof_info_msg.mode = std::string(std::begin(tof_info.mode), std::end(tof_info.mode)); + tof_info_msg.imager = std::string(std::begin(tof_info.imager), std::end(tof_info.imager)); + + tof_info_msg.extrinsics.header = header; + tof_info_msg.extrinsics.tx = tof_info.extrinsic_optic_to_user.trans_x; + tof_info_msg.extrinsics.ty = tof_info.extrinsic_optic_to_user.trans_y; + tof_info_msg.extrinsics.tz = tof_info.extrinsic_optic_to_user.trans_z; + tof_info_msg.extrinsics.rot_x = tof_info.extrinsic_optic_to_user.rot_x; + tof_info_msg.extrinsics.rot_y = tof_info.extrinsic_optic_to_user.rot_y; + tof_info_msg.extrinsics.rot_z = tof_info.extrinsic_optic_to_user.rot_z; + + tof_info_msg.intrinsics.header = header; + tof_info_msg.intrinsics.model_id = tof_info.intrinsic_calibration.model_id; + tof_info_msg.intrinsics.model_parameters = tof_info.intrinsic_calibration.model_parameters; + + tof_info_msg.inverse_intrinsics.header = header; + tof_info_msg.inverse_intrinsics.model_id = tof_info.inverse_intrinsic_calibration.model_id; + tof_info_msg.inverse_intrinsics.model_parameters = tof_info.inverse_intrinsic_calibration.model_parameters; + } + catch (...) + { + RCLCPP_ERROR(logger, "Failed to read tof info."); + } + + return tof_info_msg; +} + +ifm3d_ros2::msg::TOFInfo ifm3d_to_tof_info(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) + +{ + return ifm3d_to_tof_info(buffer, header, logger); +} + +rclcpp::Time ifm3d_to_ros_time(const TimePointT& time_point) +{ + auto duration = time_point.time_since_epoch(); + int64_t nanoseconds = std::chrono::duration_cast(duration).count(); + return rclcpp::Time(nanoseconds, RCL_SYSTEM_TIME); +} + +geometry_msgs::msg::TransformStamped trans_rot_to_optical_mount_link(std::vector trans_rot, + std::uint64_t timestamp, + std::string mounting_frame_name, + std::string optical_frame_name) +{ + /* + * tf2::Quaternion assumes extrinsic euler angles as roll pitch yaw for setRPY(), + * meaning all three rotations happen in relation to a fixed coordinate system. + * But the RPY angles from ifm3d are performing an intrinsic euler rotation, + * the reference coordinate system is updated after each individual rotation. + * Therefore, we split the received rotation into three different quaternions + * and perform the rotations in R->P->Y order. + */ + tf2::Quaternion q_roll, q_pitch, q_yaw, q_combined; + q_roll.setRPY(trans_rot[3], 0.0, 0.0); + q_pitch.setRPY(0.0, trans_rot[4], 0.0); + q_yaw.setRPY(0.0, 0.0, trans_rot[5]); + q_combined = q_roll * q_pitch * q_yaw; + + geometry_msgs::msg::TransformStamped t; + t.header.stamp = rclcpp::Time(timestamp); + t.header.frame_id = mounting_frame_name; + t.child_frame_id = optical_frame_name; + t.transform.translation.x = trans_rot[0]; + t.transform.translation.y = trans_rot[1]; + t.transform.translation.z = trans_rot[2]; + t.transform.rotation.x = q_combined.x(); + t.transform.rotation.y = q_combined.y(); + t.transform.rotation.z = q_combined.z(); + t.transform.rotation.w = q_combined.w(); + return t; +} + +bool ifm3d_extrinsic_opt_to_user_to_optical_mount_link(ifm3d::calibration::ExtrinsicOpticToUser opt_to_user, + std::uint64_t timestamp, std::string mounting_frame_name, + std::string optical_frame_name, const rclcpp::Logger& logger, + geometry_msgs::msg::TransformStamped& tf) +{ + try + { + std::vector trans_rot = { opt_to_user.trans_x, opt_to_user.trans_y, opt_to_user.trans_z, + opt_to_user.rot_x, opt_to_user.rot_y, opt_to_user.rot_z }; + tf = trans_rot_to_optical_mount_link(trans_rot, timestamp, mounting_frame_name, optical_frame_name); + return true; + } + catch (...) + { + RCLCPP_ERROR(logger, "Failed to read tof info."); + return false; + } +} + +bool ifm3d_rgb_info_to_optical_mount_link(ifm3d::Buffer& buffer, std::string mounting_frame_name, + std::string optical_frame_name, const rclcpp::Logger& logger, + geometry_msgs::msg::TransformStamped& tf) +{ + try + { + auto rgb_info = ifm3d::RGBInfoV1::Deserialize(buffer); + return ifm3d_extrinsic_opt_to_user_to_optical_mount_link(rgb_info.extrinsic_optic_to_user, rgb_info.timestamp_ns, + mounting_frame_name, optical_frame_name, logger, tf); + } + catch (...) + { + RCLCPP_ERROR(logger, "Failed to read rgb info."); + return false; + } +} + +bool ifm3d_rgb_info_to_optical_mount_link(ifm3d::Buffer&& buffer, std::string mounting_frame_name, + std::string optical_frame_name, const rclcpp::Logger& logger, + geometry_msgs::msg::TransformStamped& tf) +{ + return ifm3d_rgb_info_to_optical_mount_link(buffer, mounting_frame_name, optical_frame_name, logger, tf); +} + +bool ifm3d_tof_info_to_optical_mount_link(ifm3d::Buffer& buffer, std::string mounting_frame_name, + std::string optical_frame_name, const rclcpp::Logger& logger, + geometry_msgs::msg::TransformStamped& tf) +{ + try + { + auto tof_info = ifm3d::TOFInfoV4::Deserialize(buffer); + return ifm3d_extrinsic_opt_to_user_to_optical_mount_link(tof_info.extrinsic_optic_to_user, + tof_info.exposure_timestamps_ns[0], mounting_frame_name, + optical_frame_name, logger, tf); + } + catch (...) + { + RCLCPP_ERROR(logger, "Failed to read tof info."); + return false; + } +} + +bool ifm3d_tof_info_to_optical_mount_link(ifm3d::Buffer&& buffer, std::string mounting_frame_name, + std::string optical_frame_name, const rclcpp::Logger& logger, + geometry_msgs::msg::TransformStamped& tf) +{ + return ifm3d_tof_info_to_optical_mount_link(buffer, mounting_frame_name, optical_frame_name, logger, tf); +} + +} // namespace ifm3d_ros2 diff --git a/src/lib/buffer_id_utils.cpp b/src/lib/buffer_id_utils.cpp new file mode 100644 index 0000000..151de2e --- /dev/null +++ b/src/lib/buffer_id_utils.cpp @@ -0,0 +1,250 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#include +#include +#include + +#include +#include +#include + +#include + +namespace ifm3d_ros2 +{ +namespace buffer_id_utils +{ +std::map buffer_id_map = { + { "RADIAL_DISTANCE_IMAGE", ifm3d::buffer_id::RADIAL_DISTANCE_IMAGE }, + { "NORM_AMPLITUDE_IMAGE", ifm3d::buffer_id::NORM_AMPLITUDE_IMAGE }, + { "AMPLITUDE_IMAGE", ifm3d::buffer_id::AMPLITUDE_IMAGE }, + { "GRAYSCALE_IMAGE", ifm3d::buffer_id::GRAYSCALE_IMAGE }, + { "RADIAL_DISTANCE_NOISE", ifm3d::buffer_id::RADIAL_DISTANCE_NOISE }, + { "REFLECTIVITY", ifm3d::buffer_id::REFLECTIVITY }, + { "CARTESIAN_X_COMPONENT", ifm3d::buffer_id::CARTESIAN_X_COMPONENT }, + { "CARTESIAN_Y_COMPONENT", ifm3d::buffer_id::CARTESIAN_Y_COMPONENT }, + { "CARTESIAN_Z_COMPONENT", ifm3d::buffer_id::CARTESIAN_Z_COMPONENT }, + { "CARTESIAN_ALL", ifm3d::buffer_id::CARTESIAN_ALL }, + { "UNIT_VECTOR_ALL", ifm3d::buffer_id::UNIT_VECTOR_ALL }, + { "MONOCHROM_2D_12BIT", ifm3d::buffer_id::MONOCHROM_2D_12BIT }, + { "MONOCHROM_2D", ifm3d::buffer_id::MONOCHROM_2D }, + { "JPEG_IMAGE", ifm3d::buffer_id::JPEG_IMAGE }, + { "CONFIDENCE_IMAGE", ifm3d::buffer_id::CONFIDENCE_IMAGE }, + { "DIAGNOSTIC", ifm3d::buffer_id::DIAGNOSTIC }, + { "JSON_DIAGNOSTIC", ifm3d::buffer_id::JSON_DIAGNOSTIC }, + { "EXTRINSIC_CALIB", ifm3d::buffer_id::EXTRINSIC_CALIB }, + { "INTRINSIC_CALIB", ifm3d::buffer_id::INTRINSIC_CALIB }, + { "INVERSE_INTRINSIC_CALIBRATION", ifm3d::buffer_id::INVERSE_INTRINSIC_CALIBRATION }, + { "TOF_INFO", ifm3d::buffer_id::TOF_INFO }, + { "RGB_INFO", ifm3d::buffer_id::RGB_INFO }, + { "JSON_MODEL", ifm3d::buffer_id::JSON_MODEL }, + { "ALGO_DEBUG", ifm3d::buffer_id::ALGO_DEBUG }, + { "O3R_ODS_OCCUPANCY_GRID", ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID }, + { "O3R_ODS_INFO", ifm3d::buffer_id::O3R_ODS_INFO }, + { "XYZ", ifm3d::buffer_id::XYZ }, + { "EXPOSURE_TIME", ifm3d::buffer_id::EXPOSURE_TIME }, + { "ILLUMINATION_TEMP", ifm3d::buffer_id::ILLUMINATION_TEMP } +}; + +std::multimap data_stream_type_map = { + { ifm3d::buffer_id::RADIAL_DISTANCE_IMAGE, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::NORM_AMPLITUDE_IMAGE, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::AMPLITUDE_IMAGE, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::GRAYSCALE_IMAGE, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::RADIAL_DISTANCE_NOISE, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::REFLECTIVITY, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::CARTESIAN_X_COMPONENT, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::CARTESIAN_Y_COMPONENT, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::CARTESIAN_Z_COMPONENT, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::CARTESIAN_ALL, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::UNIT_VECTOR_ALL, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::MONOCHROM_2D_12BIT, ifm3d_ros2::buffer_id_utils::data_stream_type::rgb_2d }, + { ifm3d::buffer_id::MONOCHROM_2D, ifm3d_ros2::buffer_id_utils::data_stream_type::rgb_2d }, + { ifm3d::buffer_id::JPEG_IMAGE, ifm3d_ros2::buffer_id_utils::data_stream_type::rgb_2d }, + { ifm3d::buffer_id::CONFIDENCE_IMAGE, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::DIAGNOSTIC, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::DIAGNOSTIC, ifm3d_ros2::buffer_id_utils::data_stream_type::rgb_2d }, + { ifm3d::buffer_id::JSON_DIAGNOSTIC, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::JSON_DIAGNOSTIC, ifm3d_ros2::buffer_id_utils::data_stream_type::rgb_2d }, + { ifm3d::buffer_id::EXTRINSIC_CALIB, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::INTRINSIC_CALIB, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::INVERSE_INTRINSIC_CALIBRATION, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::TOF_INFO, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::RGB_INFO, ifm3d_ros2::buffer_id_utils::data_stream_type::rgb_2d }, + { ifm3d::buffer_id::JSON_MODEL, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::ALGO_DEBUG, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID, ifm3d_ros2::buffer_id_utils::data_stream_type::ods }, + { ifm3d::buffer_id::O3R_ODS_INFO, ifm3d_ros2::buffer_id_utils::data_stream_type::ods }, + { ifm3d::buffer_id::XYZ, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::EXPOSURE_TIME, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d }, + { ifm3d::buffer_id::ILLUMINATION_TEMP, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d } +}; + +std::map message_type_map = { + { ifm3d::buffer_id::RADIAL_DISTANCE_IMAGE, ifm3d_ros2::buffer_id_utils::message_type::raw_image }, + { ifm3d::buffer_id::NORM_AMPLITUDE_IMAGE, ifm3d_ros2::buffer_id_utils::message_type::raw_image }, + { ifm3d::buffer_id::AMPLITUDE_IMAGE, ifm3d_ros2::buffer_id_utils::message_type::raw_image }, + { ifm3d::buffer_id::GRAYSCALE_IMAGE, ifm3d_ros2::buffer_id_utils::message_type::not_implemented }, + { ifm3d::buffer_id::RADIAL_DISTANCE_NOISE, ifm3d_ros2::buffer_id_utils::message_type::not_implemented }, + { ifm3d::buffer_id::REFLECTIVITY, ifm3d_ros2::buffer_id_utils::message_type::not_implemented }, + { ifm3d::buffer_id::CARTESIAN_X_COMPONENT, ifm3d_ros2::buffer_id_utils::message_type::not_implemented }, + { ifm3d::buffer_id::CARTESIAN_Y_COMPONENT, ifm3d_ros2::buffer_id_utils::message_type::not_implemented }, + { ifm3d::buffer_id::CARTESIAN_Z_COMPONENT, ifm3d_ros2::buffer_id_utils::message_type::not_implemented }, + { ifm3d::buffer_id::CARTESIAN_ALL, ifm3d_ros2::buffer_id_utils::message_type::not_implemented }, + { ifm3d::buffer_id::UNIT_VECTOR_ALL, ifm3d_ros2::buffer_id_utils::message_type::not_implemented }, + { ifm3d::buffer_id::MONOCHROM_2D_12BIT, ifm3d_ros2::buffer_id_utils::message_type::not_implemented }, + { ifm3d::buffer_id::MONOCHROM_2D, ifm3d_ros2::buffer_id_utils::message_type::not_implemented }, + { ifm3d::buffer_id::JPEG_IMAGE, ifm3d_ros2::buffer_id_utils::message_type::compressed_image }, + { ifm3d::buffer_id::CONFIDENCE_IMAGE, ifm3d_ros2::buffer_id_utils::message_type::raw_image }, + { ifm3d::buffer_id::DIAGNOSTIC, ifm3d_ros2::buffer_id_utils::message_type::not_implemented }, + { ifm3d::buffer_id::JSON_DIAGNOSTIC, ifm3d_ros2::buffer_id_utils::message_type::not_implemented }, + { ifm3d::buffer_id::EXTRINSIC_CALIB, ifm3d_ros2::buffer_id_utils::message_type::extrinsics }, + { ifm3d::buffer_id::INTRINSIC_CALIB, ifm3d_ros2::buffer_id_utils::message_type::intrinsics }, + { ifm3d::buffer_id::INVERSE_INTRINSIC_CALIBRATION, ifm3d_ros2::buffer_id_utils::message_type::inverse_intrinsics }, + { ifm3d::buffer_id::TOF_INFO, ifm3d_ros2::buffer_id_utils::message_type::tof_info }, + { ifm3d::buffer_id::RGB_INFO, ifm3d_ros2::buffer_id_utils::message_type::rgb_info }, + { ifm3d::buffer_id::JSON_MODEL, ifm3d_ros2::buffer_id_utils::message_type::not_implemented }, + { ifm3d::buffer_id::ALGO_DEBUG, ifm3d_ros2::buffer_id_utils::message_type::not_implemented }, + { ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID, ifm3d_ros2::buffer_id_utils::message_type::occupancy_grid }, + { ifm3d::buffer_id::O3R_ODS_INFO, ifm3d_ros2::buffer_id_utils::message_type::zones }, + { ifm3d::buffer_id::XYZ, ifm3d_ros2::buffer_id_utils::message_type::pointcloud }, + { ifm3d::buffer_id::EXPOSURE_TIME, ifm3d_ros2::buffer_id_utils::message_type::not_implemented }, + { ifm3d::buffer_id::ILLUMINATION_TEMP, ifm3d_ros2::buffer_id_utils::message_type::not_implemented } +}; + +std::map topic_name_map = { + { ifm3d::buffer_id::RADIAL_DISTANCE_IMAGE, "distance" }, + { ifm3d::buffer_id::NORM_AMPLITUDE_IMAGE, "amplitude" }, + { ifm3d::buffer_id::AMPLITUDE_IMAGE, "raw_amplitude" }, + { ifm3d::buffer_id::GRAYSCALE_IMAGE, "GRAYSCALE_IMAGE" }, + { ifm3d::buffer_id::RADIAL_DISTANCE_NOISE, "RADIAL_DISTANCE_NOISE" }, + { ifm3d::buffer_id::REFLECTIVITY, "REFLECTIVITY" }, + { ifm3d::buffer_id::CARTESIAN_X_COMPONENT, "CARTESIAN_X_COMPONENT" }, + { ifm3d::buffer_id::CARTESIAN_Y_COMPONENT, "CARTESIAN_Y_COMPONENT" }, + { ifm3d::buffer_id::CARTESIAN_Z_COMPONENT, "CARTESIAN_Z_COMPONENT" }, + { ifm3d::buffer_id::CARTESIAN_ALL, "CARTESIAN_ALL" }, + { ifm3d::buffer_id::UNIT_VECTOR_ALL, "UNIT_VECTOR_ALL" }, + { ifm3d::buffer_id::MONOCHROM_2D_12BIT, "MONOCHROM_2D_12BIT" }, + { ifm3d::buffer_id::MONOCHROM_2D, "MONOCHROM_2D" }, + { ifm3d::buffer_id::JPEG_IMAGE, "rgb" }, + { ifm3d::buffer_id::CONFIDENCE_IMAGE, "confidence" }, + { ifm3d::buffer_id::DIAGNOSTIC, "DIAGNOSTIC" }, + { ifm3d::buffer_id::JSON_DIAGNOSTIC, "JSON_DIAGNOSTIC" }, + { ifm3d::buffer_id::EXTRINSIC_CALIB, "extrinsics" }, + { ifm3d::buffer_id::INTRINSIC_CALIB, "intrinsic_calib" }, + { ifm3d::buffer_id::INVERSE_INTRINSIC_CALIBRATION, "inverse_intrinsic_calibration" }, + { ifm3d::buffer_id::TOF_INFO, "tof_info" }, + { ifm3d::buffer_id::RGB_INFO, "rgb_info" }, + { ifm3d::buffer_id::JSON_MODEL, "JSON_MODEL" }, + { ifm3d::buffer_id::ALGO_DEBUG, "ALGO_DEBUG" }, + { ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID, "ods_occupancy_grid" }, + { ifm3d::buffer_id::O3R_ODS_INFO, "ods_info" }, + { ifm3d::buffer_id::XYZ, "cloud" }, + { ifm3d::buffer_id::EXPOSURE_TIME, "EXPOSURE_TIME" }, + { ifm3d::buffer_id::ILLUMINATION_TEMP, "ILLUMINATION_TEMP" } +}; + +bool convert(const std::string& string, ifm3d::buffer_id& buffer_id) +{ + if (!buffer_id_map.count(string)) + { + // key does not exist + return false; + } + + buffer_id = buffer_id_map[string]; + return true; +} + +bool convert(const ifm3d::buffer_id& buffer_id, std::string& string) +{ + // Iterate over all map entries + for (auto const& [key, value] : buffer_id_map) + { + if (value == buffer_id) + { + string = key; + return true; + } + } + + // buffer_id not found + return false; +} + +std::vector buffer_ids_for_data_stream_type(const std::vector& input_ids, + const ifm3d_ros2::buffer_id_utils::data_stream_type& type) +{ + typedef std::multimap::iterator mm_iterator; + + std::vector ret_vector; + + for (ifm3d::buffer_id input_id : input_ids) + { + // Get iterators for multimap subsection of given buffer_id + // Remember, multimaps are always sorted by their key + std::pair result = + ifm3d_ros2::buffer_id_utils::data_stream_type_map.equal_range(input_id); + + // Look for matching data_streamtype, iterating over the subsection + for (mm_iterator it = result.first; it != result.second; it++) + { + if (it->second == type) + { + ret_vector.push_back(input_id); + } + } + } + + return ret_vector; +} + +std::string vector_to_string(const std::vector& vector) +{ + if (vector.empty()) + { + return std::string(""); + } + + std::ostringstream stream; + const std::string delimiter = ", "; + + std::copy(vector.begin(), vector.end(), std::ostream_iterator(stream, delimiter.c_str())); + + const std::string& output = stream.str(); + + return output.substr(0, output.length() - delimiter.length()); +} + +/** + * Helper to create one string from a vector of buffer_ids + */ +std::string vector_to_string(const std::vector& vector) +{ + if (vector.empty()) + { + return std::string(""); + } + + std::ostringstream stream; + const std::string delimiter = ", "; + + for (const auto& id : vector) + { + std::string string; + convert(id, string); + stream << string << delimiter; + } + + const std::string& output = stream.str(); + + return output.substr(0, output.length() - delimiter.length()); +} + +} // namespace buffer_id_utils + +} // namespace ifm3d_ros2 diff --git a/src/lib/camera_node.cpp b/src/lib/camera_node.cpp index e8aba47..0412d19 100644 --- a/src/lib/camera_node.cpp +++ b/src/lib/camera_node.cpp @@ -1,27 +1,28 @@ /* * SPDX-License-Identifier: Apache-2.0 - * Copyright (C) 2019 ifm electronic, gmbh + * Copyright (C) 2024 ifm electronic, gmbh */ +#include #include #include #include #include #include +#include #include #include #include #include #include +#include +#include #include -#include #include #include -#include -#include #include using json = ifm3d::json; @@ -29,18 +30,12 @@ using namespace std::chrono_literals; namespace ifm3d_ros2 { -namespace -{ -constexpr auto xmlrpc_base_port = 50010; - -} // namespace - CameraNode::CameraNode(const rclcpp::NodeOptions& opts) : CameraNode::CameraNode("camera", opts) { } CameraNode::CameraNode(const std::string& node_name, const rclcpp::NodeOptions& opts) - : rclcpp_lifecycle::LifecycleNode(node_name, "", opts), logger_(this->get_logger()), width_(0), height_(0) + : rclcpp_lifecycle::LifecycleNode(node_name, "", opts), logger_(this->get_logger()) { // unbuffered I/O to stdout (so we can see our log messages) std::setvbuf(stdout, nullptr, _IONBF, BUFSIZ); @@ -48,9 +43,6 @@ CameraNode::CameraNode(const std::string& node_name, const rclcpp::NodeOptions& RCLCPP_INFO(this->logger_, "node name: %s", this->get_name()); RCLCPP_INFO(this->logger_, "middleware: %s", rmw_get_implementation_identifier()); - hardware_id_ = std::string(get_namespace()) + "/" + std::string(get_name()); - RCLCPP_INFO(logger_, "hardware_id: %s", hardware_id_.c_str()); - // declare our parameters and default values -- parameters defined in // the passed in `opts` (via __params:=/path/to/params.yaml on cmd line) // will override our default values specified. @@ -58,24 +50,7 @@ CameraNode::CameraNode(const std::string& node_name, const rclcpp::NodeOptions& this->init_params(); RCLCPP_INFO(this->logger_, "After the parameters declaration"); - // - // Set up our service servers - // - this->dump_srv_ = - this->create_service("~/Dump", std::bind(&ifm3d_ros2::CameraNode::Dump, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3)); - - this->config_srv_ = this->create_service( - "~/Config", std::bind(&ifm3d_ros2::CameraNode::Config, this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); - - this->soft_off_srv_ = this->create_service( - "~/Softoff", std::bind(&ifm3d_ros2::CameraNode::Softoff, this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); - - this->soft_on_srv_ = this->create_service( - "~/Softon", std::bind(&ifm3d_ros2::CameraNode::Softon, this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); + this->gil_ = std::make_shared(); RCLCPP_INFO(this->logger_, "node created, waiting for `configure()`..."); } @@ -106,65 +81,110 @@ TC_RETVAL CameraNode::on_configure(const rclcpp_lifecycle::State& prev_state) } // - // Set up our publishers. + // We need a global lock on all the ifm3d core data structures + // + std::lock_guard lock(*this->gil_); + + // + // Initialize the camera interface // - this->initialize_publishers(); - RCLCPP_INFO(this->logger_, "After publishers declaration"); + RCLCPP_INFO(this->logger_, "Initializing camera..."); + this->o3r_ = std::make_shared(this->ip_, this->xmlrpc_port_); + RCLCPP_INFO(this->logger_, "Initializing FrameGrabber"); + this->fg_ = std::make_shared(this->o3r_, this->pcic_port_); + // We do not use the asynchronous diagnostic right now, so no need to declare + // the diagnostic framegrabber + // this->fg_diag_ = std::make_shared(this->o3r_, 50009); + + if (this->config_file_!=""){ + std::ifstream file(this->config_file_); + if (!file.is_open()) { + throw std::runtime_error("Could not open config file: " + this->config_file_); + } + std::stringstream buffer; + buffer << file.rdbuf(); + RCLCPP_INFO(this->logger_, "Setting configuration: %s", buffer.str().c_str()); + ifm3d::json config_json = json::parse(buffer.str()) ; + this->o3r_->Set(config_json); + } + + // Get all the necessary info for the port. + for (auto port : this->o3r_->Ports()) + { + if (port.pcic_port == this->pcic_port_) + { + this->port_info_ = port; + } + } + + // Get resolution from port configuration + std::string j_string = "/ports/" + this->port_info_.port + "/info/features/resolution"; + ifm3d::json::json_pointer j(j_string); + auto resolution = this->o3r_->Get({ j_string })[j]; + auto width = static_cast(resolution["width"]); + auto height = static_cast(resolution["height"]); + + // Determine data stream type from port info + this->data_stream_type_ = stream_type_from_port_info(this->port_info_); - // Publish static transform for optical link - publish_optical_link_transform(); + // Create RGB or 3D modules depending on data type + if (this->data_stream_type_ == ifm3d_ros2::buffer_id_utils::data_stream_type::rgb_2d) + { + RCLCPP_INFO(logger_, "Data type is 2D"); + this->data_module_ = + std::make_shared(this->get_logger(), shared_from_this(), o3r_, this->port_info_.port, width, height); + this->buffer_list_.insert(this->buffer_list_.end(), + std::get>(this->data_module_)->buffer_id_list_.begin(), + std::get>(this->data_module_)->buffer_id_list_.end()); + this->modules_.push_back(std::get>(this->data_module_)); + RCLCPP_DEBUG(this->logger_, "RgbModule created."); + } + else if (this->data_stream_type_ == ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d) + { + RCLCPP_INFO(logger_, "Data type is 3D"); + this->data_module_ = + std::make_shared(this->get_logger(), shared_from_this(), o3r_, this->port_info_.port, width, height); + this->buffer_list_.insert(this->buffer_list_.end(), + std::get>(this->data_module_)->buffer_id_list_.begin(), + std::get>(this->data_module_)->buffer_id_list_.end()); + this->modules_.push_back(std::get>(this->data_module_)); + RCLCPP_DEBUG(this->logger_, "TofModule created."); + } + else + { + RCLCPP_ERROR(this->logger_, "Unknown data stream type"); + return TC_RETVAL::ERROR; + } // - // We need a global lock on all the ifm3d core data structures + // Initialize diagnostic Module // - std::lock_guard lock(this->gil_); + RCLCPP_DEBUG(this->logger_, "Creating DiagModule..."); + this->diag_module_ = std::make_shared(this->get_logger(), shared_from_this(), this->o3r_); + RCLCPP_DEBUG(this->logger_, "DiagModule created."); // - // Initialize the camera interface + // Create a list of all the modules to reduce duplicate code // + this->modules_.push_back(this->diag_module_); - RCLCPP_INFO(this->logger_, "Initializing camera..."); - this->cam_ = std::make_shared(this->ip_, this->xmlrpc_port_); - RCLCPP_INFO(this->logger_, "Initializing FrameGrabber"); - this->fg_ = std::make_shared(this->cam_, this->pcic_port_); - this->fg_diag_ = std::make_shared(this->cam_, 50009); - // Get PortInfo from Camera to determine data stream type - auto ports = this->cam_->Ports(); - this->data_stream_type_ = stream_type_from_port_info(ports, this->pcic_port_); - - // Remove buffer_ids unfit for the given Port - this->buffer_id_list_ = - buffer_id_utils::buffer_ids_for_data_stream_type(this->buffer_id_list_, this->data_stream_type_); - RCLCPP_INFO(logger_, "After removing buffer_ids unfit for the given data stream type, the final list is: [%s].", - buffer_id_utils::vector_to_string(this->buffer_id_list_).c_str()); - - // Timer to pull diagnostics data from ifm3d - RCLCPP_INFO(logger_, "Registering timer to pull diagnostics..."); - diagnostic_timer_ = this->create_wall_timer(1s, [this]() { - std::lock_guard lock(this->gil_); - if (this->diag_mode_=="periodic"){ - try{ - ifm3d::json diagnostic_json = cam_->GetDiagnostic(); - RCLCPP_DEBUG(this->get_logger(), "Diagnostics: %s", diagnostic_json.dump().c_str()); - - DiagnosticArrayMsg msg; - msg.header.stamp = get_clock()->now(); - auto events = diagnostic_json["events"]; - for (auto event : events) - { - msg.status.push_back(create_diagnostic_status(diagnostic_msgs::msg::DiagnosticStatus::OK, event.dump())); - } - diagnostic_publisher_->publish(msg); - } - catch (const ifm3d::Error& ex){ - RCLCPP_INFO(this->get_logger(), "ifm3d error while trying to get the diagnostic: %s", ex.what()); - } - catch (...){ - RCLCPP_INFO(this->get_logger(), "Unknown error while trying to get the diagnostic"); - } + // Transition function modules + for (auto module : this->modules_) + { + auto retval = module->on_configure(prev_state); + if (retval != TC_RETVAL::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Module %s transition did not succeed.", module->get_name().c_str()); + // TODO de-transition previous modules + return retval; } - }); - diagnostic_timer_->cancel(); // Deactivate timer, manage activity via lifecycle + } + + // Initialize the services using the BaseServices class + RCLCPP_INFO(this->logger_, "Creating BaseServices..."); + this->base_services_ = + std::make_shared(this->get_logger(), shared_from_this(), this->o3r_, this->port_info_, this->gil_); + RCLCPP_INFO(this->logger_, "BaseServices created."); RCLCPP_INFO(this->logger_, "Configuration complete."); return TC_RETVAL::SUCCESS; @@ -175,29 +195,36 @@ TC_RETVAL CameraNode::on_activate(const rclcpp_lifecycle::State& prev_state) RCLCPP_INFO(this->logger_, "on_activate(): %s -> %s", prev_state.label().c_str(), this->get_current_state().label().c_str()); - // activate all publishers - RCLCPP_INFO(this->logger_, "Activating publishers..."); - this->activate_publishers(); - RCLCPP_INFO(this->logger_, "Publishers activated."); - - // Start the Framegrabber, needs a BufferList (a vector of std::variant) - RCLCPP_INFO(this->logger_, "Starting the Framegrabber..."); - std::vector> buffer_list{}; - buffer_list.insert(buffer_list.end(), buffer_id_list_.begin(), buffer_id_list_.end()); - // Start framegrabber and wait for the returned future - this->fg_->Start(buffer_list).wait(); // Register Callbacks to handle new frames and print errors this->fg_->OnNewFrame(std::bind(&CameraNode::frame_callback, this, std::placeholders::_1)); this->fg_->OnError(std::bind(&CameraNode::error_callback, this, std::placeholders::_1)); - this->fg_diag_->OnAsyncError(std::bind(&CameraNode::async_error_callback, this, std::placeholders::_1, - std::placeholders::_2)); - this->fg_diag_->OnAsyncNotification(std::bind(&CameraNode::async_notification_callback, this, - std::placeholders::_1, std::placeholders::_2)); - this->fg_diag_->Start({}).wait(); + + // Registering the error and notification callbacks is currently not necessary, + // as currently we only use the periodic diagnostic poll. + // this->fg_diag_->OnAsyncError( + // std::bind(&CameraNode::async_error_callback, this, std::placeholders::_1, std::placeholders::_2)); + // this->fg_diag_->OnAsyncNotification( + // std::bind(&CameraNode::async_notification_callback, this, std::placeholders::_1, std::placeholders::_2)); + + // Start the Framegrabber, needs a BufferList (a vector of std::variant) + RCLCPP_INFO(this->logger_, "Starting the Framegrabber..."); + this->fg_->Start(this->buffer_list_).wait(); + // Currently, the diagnostic framegrabber is not used as we are only using the + // periodic diagnostic + // this->fg_diag_->Start({}).wait(); RCLCPP_INFO(this->logger_, "Framegrabber started."); - diagnostic_timer_->reset(); - RCLCPP_INFO(this->logger_, "Diagnostic monitoring active."); + // Transition function modules + for (auto module : this->modules_) + { + auto retval = module->on_activate(prev_state); + if (retval != TC_RETVAL::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Module %s transition did not succeed.", module->get_name().c_str()); + // TODO de-transition previous modules + return retval; + } + } return TC_RETVAL::SUCCESS; } @@ -207,16 +234,20 @@ TC_RETVAL CameraNode::on_deactivate(const rclcpp_lifecycle::State& prev_state) RCLCPP_INFO(this->logger_, "on_deactivate(): %s -> %s", prev_state.label().c_str(), this->get_current_state().label().c_str()); - diagnostic_timer_->cancel(); - RCLCPP_INFO(this->logger_, "Diagnostic monitoring inactive."); - - RCLCPP_INFO(logger_, "Stopping Framebuffer..."); + RCLCPP_INFO(logger_, "Stopping FrameGrabber..."); this->fg_->Stop().wait(); - // explicitly deactive the publishers - RCLCPP_INFO(this->logger_, "Deactivating publishers..."); - this->deactivate_publishers(); - RCLCPP_INFO(this->logger_, "Publishers deactivated."); + // Transition function modules + for (auto module : this->modules_) + { + auto retval = module->on_deactivate(prev_state); + if (retval != TC_RETVAL::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Module %s transition did not succeed.", module->get_name().c_str()); + // TODO de-transition previous modules + return retval; + } + } return TC_RETVAL::SUCCESS; } @@ -227,11 +258,23 @@ TC_RETVAL CameraNode::on_cleanup(const rclcpp_lifecycle::State& prev_state) RCLCPP_INFO(this->logger_, "on_cleanup(): %s -> %s", prev_state.label().c_str(), this->get_current_state().label().c_str()); - std::lock_guard lock(this->gil_); + std::lock_guard lock(*this->gil_); RCLCPP_INFO(this->logger_, "Resetting core ifm3d data structures..."); this->fg_.reset(); - this->cam_.reset(); + this->o3r_.reset(); + + // Transition function modules + for (auto module : this->modules_) + { + auto retval = module->on_cleanup(prev_state); + if (retval != TC_RETVAL::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Module %s transition did not succeed.", module->get_name().c_str()); + // TODO de-transition previous modules + return retval; + } + } RCLCPP_INFO(this->logger_, "Node cleanup complete."); @@ -243,6 +286,22 @@ TC_RETVAL CameraNode::on_shutdown(const rclcpp_lifecycle::State& prev_state) RCLCPP_INFO(this->logger_, "on_shutdown(): %s -> %s", prev_state.label().c_str(), this->get_current_state().label().c_str()); + // TODO: figure out how to properly shutdown modules + this->fg_->Stop(); + // this->fg_diag_->Stop(); + + // Transition function modules + for (auto module : this->modules_) + { + auto retval = module->on_shutdown(prev_state); + if (retval != TC_RETVAL::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Module %s transition did not succeed.", module->get_name().c_str()); + // TODO de-transition previous modules + return retval; + } + } + return TC_RETVAL::SUCCESS; } @@ -251,11 +310,23 @@ TC_RETVAL CameraNode::on_error(const rclcpp_lifecycle::State& prev_state) RCLCPP_INFO(this->logger_, "on_error(): %s -> %s", prev_state.label().c_str(), this->get_current_state().label().c_str()); - std::lock_guard lock(this->gil_); + std::lock_guard lock(*this->gil_); RCLCPP_INFO(this->logger_, "Resetting core ifm3d data structures..."); - // this->im_.reset(); + this->fg_.reset(); - this->cam_.reset(); + this->o3r_.reset(); + + // Transition function modules + for (auto module : this->modules_) + { + auto retval = module->on_error(prev_state); + if (retval != TC_RETVAL::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Module %s transition did not succeed.", module->get_name().c_str()); + // TODO de-transition previous modules + return retval; + } + } RCLCPP_INFO(this->logger_, "Error processing complete."); @@ -272,22 +343,11 @@ void CameraNode::init_params() * - Define Descriptor * - Declare Parameter */ - - rcl_interfaces::msg::ParameterDescriptor buffer_id_list_descriptor; - const std::vector default_buffer_id_list{ - // - "CONFIDENCE_IMAGE", // - "EXTRINSIC_CALIB", // - "JPEG_IMAGE", // - "NORM_AMPLITUDE_IMAGE", // - "RADIAL_DISTANCE_IMAGE", // - "RGB_INFO", // - "XYZ", // - }; - buffer_id_list_descriptor.name = "buffer_id_list"; - buffer_id_list_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; - buffer_id_list_descriptor.description = "List of buffer_id strings denoting the wanted buffers."; - this->declare_parameter("buffer_id_list", default_buffer_id_list, buffer_id_list_descriptor); + rcl_interfaces::msg::ParameterDescriptor config_descriptor; + config_descriptor.name = "config_file"; + config_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + config_descriptor.description = "Configuration file, in JSON format."; + this->declare_parameter("config_file", "", config_descriptor); rcl_interfaces::msg::ParameterDescriptor ip_descriptor; ip_descriptor.name = "ip"; @@ -304,52 +364,6 @@ void CameraNode::init_params() "is connected to."; this->declare_parameter("pcic_port", ifm3d::DEFAULT_PCIC_PORT, pcic_port_descriptor); - rcl_interfaces::msg::ParameterDescriptor tf_cloud_link_frame_name_descriptor; - tf_cloud_link_frame_name_descriptor.name = "tf.cloud_link.frame_name"; - tf_cloud_link_frame_name_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - tf_cloud_link_frame_name_descriptor.description = - "Name for the point cloud frame, defaults to _optical_link."; - this->declare_parameter("tf.cloud_link.frame_name", node_name + "_cloud_link", tf_cloud_link_frame_name_descriptor); - - rcl_interfaces::msg::ParameterDescriptor tf_cloud_link_publish_transform_descriptor; - tf_cloud_link_publish_transform_descriptor.name = "tf.cloud_link.publish_transform"; - tf_cloud_link_publish_transform_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - tf_cloud_link_publish_transform_descriptor.description = - "Whether the transform from the cameras mounting point to the point cloud center should be published."; - this->declare_parameter("tf.cloud_link.publish_transform", true, tf_cloud_link_publish_transform_descriptor); - - rcl_interfaces::msg::ParameterDescriptor tf_mounting_link_frame_name_descriptor; - tf_mounting_link_frame_name_descriptor.name = "tf.mounting_link.frame_name"; - tf_mounting_link_frame_name_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - tf_mounting_link_frame_name_descriptor.description = - "Name for the mounting point frame, defaults to _mounting_link."; - this->declare_parameter("tf.mounting_link.frame_name", node_name + "_mounting_link", - tf_mounting_link_frame_name_descriptor); - - rcl_interfaces::msg::ParameterDescriptor tf_optical_link_frame_name_descriptor; - tf_optical_link_frame_name_descriptor.name = "tf.optical_link.frame_name"; - tf_optical_link_frame_name_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - tf_optical_link_frame_name_descriptor.description = - "Name for the point optical frame, defaults to _optical_link."; - this->declare_parameter("tf.optical_link.frame_name", node_name + "_optical_link", - tf_optical_link_frame_name_descriptor); - - rcl_interfaces::msg::ParameterDescriptor tf_optical_link_publish_transform_descriptor; - tf_optical_link_publish_transform_descriptor.name = "tf.optical_link.publish_transform"; - tf_optical_link_publish_transform_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - tf_optical_link_publish_transform_descriptor.description = - "Whether the transform from the cameras mounting point to the point optical center should be published."; - this->declare_parameter("tf.optical_link.publish_transform", true, tf_optical_link_publish_transform_descriptor); - - rcl_interfaces::msg::ParameterDescriptor tf_optical_link_transform_descriptor; - std::vector default_transform; - default_transform.resize(6, 0.0); - tf_optical_link_transform_descriptor.name = "tf.optical_link.transform"; - tf_optical_link_transform_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; - tf_optical_link_transform_descriptor.description = - "Static transform from mounting link to optical link, as [x, y, z, rot_x, rot_y, rot_z]"; - this->declare_parameter("tf.optical_link.transform", default_transform, tf_optical_link_transform_descriptor); - rcl_interfaces::msg::ParameterDescriptor xmlrpc_port_descriptor; xmlrpc_port_descriptor.name = "xmlrpc_port"; xmlrpc_port_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; @@ -362,18 +376,8 @@ void CameraNode::init_params() xmlrpc_port_descriptor.integer_range.push_back(xmlrpc_port_range); this->declare_parameter("xmlrpc_port", ifm3d::DEFAULT_XMLRPC_PORT, xmlrpc_port_descriptor); - rcl_interfaces::msg::ParameterDescriptor diag_mode_descriptor; - diag_mode_descriptor.name = "diag_mode"; - diag_mode_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - diag_mode_descriptor.description = - "Diagnostic mode: asynchronous monitoring or periodic polling."; - this->declare_parameter("diag_mode", "async", diag_mode_descriptor); - // TODO: extend parameter description to include required params: // password: for lock / unlock of JSON configuration - - - } void CameraNode::parse_params() @@ -384,26 +388,8 @@ void CameraNode::parse_params() * - Where applicable, parse read data into more useful data type */ - std::vector buffer_id_strings; - this->get_parameter("buffer_id_list", buffer_id_strings); - RCLCPP_INFO(this->logger_, "Reading %ld buffer_ids: [%s]", buffer_id_strings.size(), - buffer_id_utils::vector_to_string(buffer_id_strings).c_str()); - // Populate buffer_id_list_ from read strings - this->buffer_id_list_.clear(); - for (const std::string& string : buffer_id_strings) - { - ifm3d::buffer_id found_id; - if (buffer_id_utils::convert(string, found_id)) - { - this->buffer_id_list_.push_back(found_id); - } - else - { - RCLCPP_WARN(this->logger_, "Ignoring unknown buffer_id %s", string.c_str()); - } - } - RCLCPP_INFO(this->logger_, "Parsed %ld buffer_ids: %s", this->buffer_id_list_.size(), - buffer_id_utils::vector_to_string(this->buffer_id_list_).c_str()); + this->get_parameter("config_file", this->config_file_); + RCLCPP_INFO(this->logger_, "Config file: %s", this->config_file_.c_str()); this->get_parameter("ip", this->ip_); RCLCPP_INFO(this->logger_, "ip: %s", this->ip_.c_str()); @@ -411,40 +397,8 @@ void CameraNode::parse_params() this->get_parameter("pcic_port", this->pcic_port_); RCLCPP_INFO(this->logger_, "pcic_port: %u", this->pcic_port_); - this->get_parameter("tf.cloud_link.frame_name", this->tf_cloud_link_frame_name_); - RCLCPP_INFO(this->logger_, "tf.cloud_link.frame_name: %s", this->tf_cloud_link_frame_name_.c_str()); - - this->get_parameter("tf.cloud_link.publish_transform", this->tf_cloud_link_publish_transform_); - RCLCPP_INFO(this->logger_, "tf.cloud_link.publish_transform: %s", - this->tf_cloud_link_publish_transform_ ? "true" : "false"); - - this->get_parameter("tf.mounting_link.frame_name", this->tf_mounting_link_frame_name_); - RCLCPP_INFO(this->logger_, "tf.mounting_link.frame_name: %s", this->tf_mounting_link_frame_name_.c_str()); - - this->get_parameter("tf.optical_link.frame_name", this->tf_optical_link_frame_name_); - RCLCPP_INFO(this->logger_, "tf.optical_link.frame_name: %s", this->tf_optical_link_frame_name_.c_str()); - - this->get_parameter("tf.optical_link.publish_transform", this->tf_optical_link_publish_transform_); - RCLCPP_INFO(this->logger_, "tf.optical_link.publish_transform: %s", - this->tf_optical_link_publish_transform_ ? "true" : "false"); - - this->get_parameter("tf.optical_link.transform", this->tf_optical_link_transform_); - if (this->tf_optical_link_transform_.size() != 6) - { - RCLCPP_WARN(logger_, "Invalid number of entries for tf.optical_link.transform: %ld. %s", - this->tf_optical_link_transform_.size(), "Using the first 6 values, filling in with 0.0 if nessesary."); - this->tf_optical_link_transform_.resize(6, 0.0); - } - RCLCPP_INFO(this->logger_, "tf.optical_link.transform: [%f, %f, %f, %f, %f, %f]", this->tf_optical_link_transform_[0], - this->tf_optical_link_transform_[1], this->tf_optical_link_transform_[2], - this->tf_optical_link_transform_[3], this->tf_optical_link_transform_[4], - this->tf_optical_link_transform_[5]); - this->get_parameter("xmlrpc_port", this->xmlrpc_port_); RCLCPP_INFO(this->logger_, "xmlrpc_port: %u", this->xmlrpc_port_); - - this->get_parameter("diag_mode", this->diag_mode_); - RCLCPP_INFO(this->logger_, "diag_mode: %s", this->diag_mode_.c_str()); } void CameraNode::set_parameter_event_callbacks() @@ -457,13 +411,10 @@ void CameraNode::set_parameter_event_callbacks() * - Create a callback as lambda to handle parameter change at runtime * - Add lambda to parameter subscriber */ - - auto buffer_id_list_cb = [this](const rclcpp::Parameter& p) { - RCLCPP_WARN(logger_, "This new buffer_id_list will be used after CONFIGURE transition was called: %s", - buffer_id_utils::vector_to_string(p.as_string_array()).c_str()); + auto config_file_cb = [this](const rclcpp::Parameter& p) { + RCLCPP_WARN(logger_, "This new config_file will be used after CONFIGURE transition was called: '%s'", p.as_string().c_str()); }; - registered_param_callbacks_["buffer_id_list"] = - param_subscriber_->add_parameter_callback("buffer_id_list", buffer_id_list_cb); + registered_param_callbacks_["config_file"] = param_subscriber_->add_parameter_callback("config_file", config_file_cb); auto ip_cb = [this](const rclcpp::Parameter& p) { RCLCPP_WARN(logger_, "This new ip will be used after CONFIGURE transition was called: '%s'", p.as_string().c_str()); @@ -475,566 +426,39 @@ void CameraNode::set_parameter_event_callbacks() }; registered_param_callbacks_["pcic_port"] = param_subscriber_->add_parameter_callback("pcic_port", pcic_port_cb); - auto tf_cloud_link_frame_name_cb = [this](const rclcpp::Parameter& p) { - this->tf_cloud_link_frame_name_ = p.as_string(); - RCLCPP_INFO(logger_, - "This new tf.cloud_link.frame_name will be used as soon as the next Extrinsics buffer is received: " - "'%s'", - this->tf_cloud_link_frame_name_.c_str()); - }; - registered_param_callbacks_["tf.cloud_link.frame_name"] = - param_subscriber_->add_parameter_callback("tf.cloud_link.frame_name", tf_cloud_link_frame_name_cb); - - auto tf_cloud_link_publish_transform_cb = [this](const rclcpp::Parameter& p) { - this->tf_cloud_link_publish_transform_ = p.as_bool(); - RCLCPP_INFO(logger_, "New tf.cloud_link.publish_transform: %s", - this->tf_cloud_link_publish_transform_ ? "true" : "false"); - }; - registered_param_callbacks_["tf.cloud_link.publish_transform"] = - param_subscriber_->add_parameter_callback("tf.cloud_link.publish_transform", tf_cloud_link_publish_transform_cb); - - auto tf_mounting_link_frame_name_cb = [this](const rclcpp::Parameter& p) { - this->tf_mounting_link_frame_name_ = p.as_string(); - publish_optical_link_transform(); - RCLCPP_INFO(logger_, "New tf.mounting_link.frame_name: '%s'", this->tf_mounting_link_frame_name_.c_str()); - }; - registered_param_callbacks_["tf.mounting_link.frame_name"] = - param_subscriber_->add_parameter_callback("tf.mounting_link.frame_name", tf_mounting_link_frame_name_cb); - - auto tf_optical_link_frame_name_cb = [this](const rclcpp::Parameter& p) { - this->tf_optical_link_frame_name_ = p.as_string(); - publish_optical_link_transform(); - RCLCPP_INFO(logger_, "New tf.optical_link.frame_name: '%s'", this->tf_optical_link_frame_name_.c_str()); - }; - registered_param_callbacks_["tf.optical_link.frame_name"] = - param_subscriber_->add_parameter_callback("tf.optical_link.frame_name", tf_optical_link_frame_name_cb); - - auto tf_optical_link_publish_transform_cb = [this](const rclcpp::Parameter& p) { - this->tf_optical_link_publish_transform_ = p.as_bool(); - publish_optical_link_transform(); - RCLCPP_INFO(logger_, "New tf.optical_link.publish_transform: %s", - this->tf_optical_link_publish_transform_ ? "true" : "false"); - }; - registered_param_callbacks_["tf.optical_link.publish_transform"] = param_subscriber_->add_parameter_callback( - "tf.optical_link.publish_transform", tf_optical_link_publish_transform_cb); - - auto tf_optical_link_transform_cb = [this](const rclcpp::Parameter& p) { - std::vector doubles_from_param = p.as_double_array(); - if (doubles_from_param.size() != 6) - { - RCLCPP_WARN(logger_, "Invalid number of entries for tf.optical_link.transform: %ld. %s", - doubles_from_param.size(), "Using the first 6 values, filling in with 0.0 if nessesary."); - doubles_from_param.resize(6, 0.0); - } - this->tf_optical_link_transform_ = doubles_from_param; - RCLCPP_INFO(this->logger_, "New tf.optical_link.transform: [%f, %f, %f, %f, %f, %f]", - this->tf_optical_link_transform_[0], this->tf_optical_link_transform_[1], - this->tf_optical_link_transform_[2], this->tf_optical_link_transform_[3], - this->tf_optical_link_transform_[4], this->tf_optical_link_transform_[5]); - publish_optical_link_transform(); - }; - registered_param_callbacks_["tf.optical_link.transform"] = - param_subscriber_->add_parameter_callback("tf.optical_link.transform", tf_optical_link_transform_cb); - auto xmlrpc_port_cb = [this](const rclcpp::Parameter& p) { this->xmlrpc_port_ = p.as_int(); RCLCPP_INFO(logger_, "New xmlrpc_port: %d", this->xmlrpc_port_); }; registered_param_callbacks_["xmlrpc_port"] = param_subscriber_->add_parameter_callback("xmlrpc_port", xmlrpc_port_cb); - - auto diag_mode_cb = [this](const rclcpp::Parameter& p) { - this->diag_mode_ = p.as_string(); - RCLCPP_INFO(logger_, "New diag_mode: %s", this->diag_mode_.c_str()); - }; - registered_param_callbacks_["diag_mode"] = param_subscriber_->add_parameter_callback("diag_mode", diag_mode_cb); -} - -void CameraNode::initialize_publishers() -{ - using namespace buffer_id_utils; - - image_publishers_.clear(); - compressed_image_publishers_.clear(); - pcl_publishers_.clear(); - extrinsics_publishers_.clear(); - - // Create static tf publisher - tf_static_broadcaster_ = std::make_shared(this); - - // Create diagnostics publisher - // Messages shall be published to global /diagnostics topic and not local one, similar to /tf - diagnostic_publisher_ = this->create_publisher("/diagnostics", 10); - - std::vector ids_to_remove{}; - - // Create correctly typed publishers for all given buffer_ids - for (const ifm3d::buffer_id& id : this->buffer_id_list_) - { - // Create Publishers in node namespace to make multi-camera setups easier - const std::string topic_name = "~/" + buffer_id_utils::topic_name_map[id]; - const auto qos = ifm3d_ros2::LowLatencyQoS(); - const buffer_id_utils::message_type message_type = buffer_id_utils::message_type_map[id]; - - switch (message_type) - { - case buffer_id_utils::message_type::raw_image: - image_publishers_[id] = this->create_publisher(topic_name, qos); - break; - case buffer_id_utils::message_type::compressed_image: - compressed_image_publishers_[id] = this->create_publisher(topic_name, qos); - break; - case buffer_id_utils::message_type::pointcloud: - pcl_publishers_[id] = this->create_publisher(topic_name, qos); - break; - case buffer_id_utils::message_type::extrinsics: - extrinsics_publishers_[id] = this->create_publisher(topic_name, qos); - break; - case buffer_id_utils::message_type::intrinsics: - intrinsics_publishers_[id] = this->create_publisher(topic_name, qos); - camera_info_publishers_[id] = this->create_publisher("~/camera_info", qos); - break; - case buffer_id_utils::message_type::rgb_info: - rgb_info_publishers_[id] = this->create_publisher(topic_name, qos); - break; - case buffer_id_utils::message_type::tof_info: - tof_info_publishers_[id] = this->create_publisher(topic_name, qos); - break; - case buffer_id_utils::message_type::inverse_intrinsics: - inverse_intrinsics_publishers_[id] = this->create_publisher(topic_name, qos); - break; - default: - std::string id_string; - convert(id, id_string); - RCLCPP_ERROR(logger_, "Unknown message type for buffer_id %s. Will be removed from list...", id_string.c_str()); - ids_to_remove.push_back(id); - break; - } - } - - // Remove all buffer_ids where type is unclear - while (ids_to_remove.size() > 0) - { - ifm3d::buffer_id id_to_remove = ids_to_remove.back(); - ids_to_remove.pop_back(); - std::vector::iterator itr = - std::find(buffer_id_list_.begin(), buffer_id_list_.end(), id_to_remove); - auto index = std::distance(buffer_id_list_.begin(), itr); - buffer_id_list_.erase(buffer_id_list_.begin() + index); - - std::string id_string; - convert(id_to_remove, id_string); - RCLCPP_INFO(logger_, "Removed buffer_id %s from list at position %ld", id_string.c_str(), index); - } -} - -void CameraNode::activate_publishers() -{ - for (auto& [id, publisher] : image_publishers_) - { - publisher->on_activate(); - } - for (auto& [id, publisher] : compressed_image_publishers_) - { - publisher->on_activate(); - } - for (auto& [id, publisher] : pcl_publishers_) - { - publisher->on_activate(); - } - for (auto& [id, publisher] : extrinsics_publishers_) - { - publisher->on_activate(); - } - for (auto& [id, publisher] : intrinsics_publishers_) - { - publisher->on_activate(); - } - for (auto& [id, publisher] : camera_info_publishers_) - { - publisher->on_activate(); - } - for (auto& [id, publisher] : tof_info_publishers_) - { - publisher->on_activate(); - } - for (auto& [id, publisher] : rgb_info_publishers_) - { - publisher->on_activate(); - } - for (auto& [id, publisher] : inverse_intrinsics_publishers_) - { - publisher->on_activate(); - } - diagnostic_publisher_->on_activate(); -}; - -void CameraNode::deactivate_publishers() -{ - for (auto& [id, publisher] : image_publishers_) - { - publisher->on_deactivate(); - } - for (auto& [id, publisher] : compressed_image_publishers_) - { - publisher->on_deactivate(); - } - for (auto& [id, publisher] : pcl_publishers_) - { - publisher->on_deactivate(); - } - for (auto& [id, publisher] : extrinsics_publishers_) - { - publisher->on_deactivate(); - } - for (auto& [id, publisher] : intrinsics_publishers_) - { - publisher->on_deactivate(); - } - for (auto& [id, publisher] : camera_info_publishers_) - { - publisher->on_deactivate(); - } - for (auto& [id, publisher] : tof_info_publishers_) - { - publisher->on_deactivate(); - } - for (auto& [id, publisher] : rgb_info_publishers_) - { - publisher->on_deactivate(); - } - for (auto& [id, publisher] : inverse_intrinsics_publishers_) - { - publisher->on_deactivate(); - } - diagnostic_publisher_->on_deactivate(); -}; - -void CameraNode::Config(const std::shared_ptr /*unused*/, ConfigRequest req, ConfigResponse resp) -{ - RCLCPP_INFO(this->logger_, "Handling config request..."); - - if (this->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - resp->status = -1; - // XXX: may want to change this logic. For now, I do it so I know - // the ifm3d data structures are not null pointers - RCLCPP_WARN(this->logger_, "Can only make a service request when node is ACTIVE"); - return; - } - - { - std::lock_guard lock(this->gil_); - resp->status = 0; - resp->msg = "OK"; - - try - { - this->cam_->FromJSON(json::parse(req->json)); // HERE - } - catch (const ifm3d::Error& ex) - { - resp->status = ex.code(); - resp->msg = ex.what(); - } - catch (const std::exception& std_ex) - { - resp->status = -1; - resp->msg = std_ex.what(); - } - catch (...) - { - resp->status = -2; - resp->msg = "Unknown error in `Config'"; - } - - if (resp->status != 0) - { - RCLCPP_WARN(this->logger_, "Config: %d - %s", resp->status, resp->msg.c_str()); - } - } - - RCLCPP_INFO(this->logger_, "Config request done."); -} - -void CameraNode::Dump(const std::shared_ptr /*unused*/, DumpRequest /*unused*/, DumpResponse resp) -{ - RCLCPP_INFO(this->logger_, "Handling dump request..."); - - if (this->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - resp->status = -1; - // XXX: may want to change this logic. For now, I do it so I know - // the ifm3d data structures are not null pointers - RCLCPP_WARN(this->logger_, "Can only make a service request when node is ACTIVE"); - return; - } - - { - std::lock_guard lock(this->gil_); - resp->status = 0; - - try - { - json j = this->cam_->ToJSON(); // HERE - resp->config = j.dump(); - } - catch (const ifm3d::Error& ex) - { - resp->status = ex.code(); - RCLCPP_WARN(this->logger_, "%s", ex.what()); - } - catch (const std::exception& std_ex) - { - resp->status = -1; - RCLCPP_WARN(this->logger_, "%s", std_ex.what()); - } - catch (...) - { - resp->status = -2; - } - - if (resp->status != 0) - { - RCLCPP_WARN(this->logger_, "Dump: %d", resp->status); - } - } - - RCLCPP_INFO(this->logger_, "Dump request done."); } -void CameraNode::Softoff(const std::shared_ptr /*unused*/, SoftoffRequest /*unused*/, - SoftoffResponse resp) +void CameraNode::frame_callback(ifm3d::Frame::Ptr frame) { - RCLCPP_INFO(this->logger_, "Handling SoftOff request..."); - - if (this->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - resp->status = -1; - RCLCPP_WARN(this->logger_, "Can only make a service request when node is ACTIVE"); - return; - } - + if (std::holds_alternative>(this->data_module_)) { - std::lock_guard lock(this->gil_); - resp->status = 0; - int port_arg = -1; - - try - { - port_arg = static_cast(this->pcic_port_) % xmlrpc_base_port; - this->cam_->FromJSONStr(R"({"ports":{"port)" + std::to_string(port_arg) + R"(": {"state": "IDLE"}}})"); - } - catch (const ifm3d::Error& ex) - { - resp->status = ex.code(); - RCLCPP_WARN(this->logger_, "%s", ex.what()); - } - catch (const std::exception& std_ex) - { - resp->status = -1; - RCLCPP_WARN(this->logger_, "%s", std_ex.what()); - } - catch (...) - { - resp->status = -2; - } - - if (resp->status != 0) - { - RCLCPP_WARN(this->logger_, "SoftOff: %d", resp->status); - } + std::get>(this->data_module_)->handle_frame(frame); } - - RCLCPP_INFO(this->logger_, "SoftOff request done."); -} - -void CameraNode::Softon(const std::shared_ptr /*unused*/, SoftonRequest /*unused*/, - SoftonResponse resp) -{ - RCLCPP_INFO(this->logger_, "Handling SoftOn request..."); - - if (this->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + else if (std::holds_alternative>(this->data_module_)) { - resp->status = -1; - RCLCPP_WARN(this->logger_, "Can only make a service request when node is ACTIVE"); - return; + std::get>(this->data_module_)->handle_frame(frame); } - - { - std::lock_guard lock(this->gil_); - resp->status = 0; - int port_arg = -1; - - try - { - port_arg = static_cast(this->pcic_port_) % xmlrpc_base_port; - this->cam_->FromJSONStr(R"({"ports":{"port)" + std::to_string(port_arg) + R"(":{"state":"RUN"}}})"); - } - catch (const ifm3d::Error& ex) - { - resp->status = ex.code(); - RCLCPP_WARN(this->logger_, "%s", ex.what()); - } - catch (const std::exception& std_ex) - { - resp->status = -1; - RCLCPP_WARN(this->logger_, "%s", std_ex.what()); - } - catch (...) - { - resp->status = -2; - } - - if (resp->status != 0) - { - RCLCPP_WARN(this->logger_, "SoftOn: %d", resp->status); - } - } - - RCLCPP_INFO(this->logger_, "SoftOn request done."); -} - -void CameraNode::frame_callback(ifm3d::Frame::Ptr frame) -{ - using namespace buffer_id_utils; - - RCLCPP_INFO_ONCE(logger_, "Receiving Frames. Processing buffer for [%s]...", - buffer_id_utils::vector_to_string(this->buffer_id_list_).c_str()); - RCLCPP_DEBUG(logger_, "Received new Frame."); - - const auto now = this->get_clock()->now(); - - auto cloud_header = std_msgs::msg::Header(); - cloud_header.frame_id = this->tf_cloud_link_frame_name_; - cloud_header.stamp = now; - - auto optical_header = std_msgs::msg::Header(); - optical_header.frame_id = this->tf_optical_link_frame_name_; - optical_header.stamp = now; - - std::lock_guard lock(this->gil_); - - for (const ifm3d::buffer_id& id : this->buffer_id_list_) - { - // Helper for logging - auto& clk = *this->get_clock(); - std::string id_string; - convert(id, id_string); - - const buffer_id_utils::message_type message_type = buffer_id_utils::message_type_map[id]; - - if (!frame->HasBuffer(id)) - { - RCLCPP_WARN_THROTTLE(logger_, clk, 5000, - "Frame does not contain buffer %s. Is the correct camera head connected?", - id_string.c_str()); - } - - switch (message_type) - { - case buffer_id_utils::message_type::raw_image: { - auto buffer = frame->GetBuffer(id); - ImageMsg raw_image_msg = ifm3d_ros2::ifm3d_to_ros_image(frame->GetBuffer(id), optical_header, logger_); - image_publishers_[id]->publish(raw_image_msg); - width_ = raw_image_msg.width; - height_ = raw_image_msg.height; - } - break; - case buffer_id_utils::message_type::compressed_image: { - auto buffer = frame->GetBuffer(id); - CompressedImageMsg compressed_image_msg = - ifm3d_ros2::ifm3d_to_ros_compressed_image(frame->GetBuffer(id), optical_header, "jpeg", logger_); - compressed_image_publishers_[id]->publish(compressed_image_msg); - } - break; - case buffer_id_utils::message_type::pointcloud: { - auto buffer = frame->GetBuffer(id); - PCLMsg pointcloud_msg = ifm3d_ros2::ifm3d_to_ros_cloud(frame->GetBuffer(id), cloud_header, logger_); - pcl_publishers_[id]->publish(pointcloud_msg); - } - break; - case buffer_id_utils::message_type::extrinsics: { - auto buffer = frame->GetBuffer(id); - ExtrinsicsMsg extrinsics_msg = ifm3d_ros2::ifm3d_to_extrinsics(buffer, optical_header, logger_); - extrinsics_publishers_[id]->publish(extrinsics_msg); - - // Set/Update mounting link to cloud link transform - publish_cloud_link_transform_if_changed(extrinsics_msg); - } - break; - case buffer_id_utils::intrinsics: { - auto buffer = frame->GetBuffer(id); - IntrinsicsMsg intrinsics_msg = ifm3d_ros2::ifm3d_to_intrinsics(buffer, optical_header, logger_); - intrinsics_publishers_[id]->publish(intrinsics_msg); - - // Also publish CameraInfo from Intrinsics - if (width_ == 0 || height_ == 0) - { - RCLCPP_WARN_THROTTLE(logger_, clk, 5000, "Needs at least one raw image buffer to parse CameraInfo!"); - } - else - { - CameraInfoMsg camera_info_msg = - ifm3d_ros2::ifm3d_to_camera_info(buffer, optical_header, height_, width_, logger_); - RCLCPP_INFO_ONCE(logger_, "Parsing CameraInfo successfull."); - camera_info_publishers_[id]->publish(camera_info_msg); - } - } - break; - case buffer_id_utils::message_type::inverse_intrinsics: { - auto buffer = frame->GetBuffer(id); - InverseIntrinsicsMsg inverse_intrinsics_msg = - ifm3d_ros2::ifm3d_to_inverse_intrinsics(buffer, optical_header, logger_); - inverse_intrinsics_publishers_[id]->publish(inverse_intrinsics_msg); - } - break; - case buffer_id_utils::message_type::tof_info: { - auto buffer = frame->GetBuffer(id); - TOFInfoMsg tof_info_msg = ifm3d_ros2::ifm3d_to_tof_info(buffer, optical_header, logger_); - tof_info_publishers_[id]->publish(tof_info_msg); - } - break; - case buffer_id_utils::message_type::rgb_info: { - auto buffer = frame->GetBuffer(id); - RGBInfoMsg rgb_info_msg = ifm3d_ros2::ifm3d_to_rgb_info(buffer, optical_header, logger_); - rgb_info_publishers_[id]->publish(rgb_info_msg); - } - break; - default: - RCLCPP_ERROR_THROTTLE(logger_, clk, 5000, "Unknown message type for buffer_id %s. Can not publish.", - id_string.c_str()); - break; - } - } - RCLCPP_DEBUG(this->logger_, "Frame callback done."); } -buffer_id_utils::data_stream_type CameraNode::stream_type_from_port_info(const std::vector& ports, - const uint16_t pcic_port) +buffer_id_utils::data_stream_type CameraNode::stream_type_from_port_info(ifm3d::PortInfo port_info) { - std::string port_type{ "" }; buffer_id_utils::data_stream_type data_stream_type; - - // Get port_type from PortInfo with matching pcic_port - for (auto port : ports) - { - RCLCPP_INFO(logger_, "Found port %s (pcic_port=%d) with type %s", port.port.c_str(), port.pcic_port, - port.type.c_str()); - if (port.pcic_port == pcic_port) - { - port_type = port.type; - break; - } - } + RCLCPP_INFO(logger_, "Using port %s (pcic_port=%d) with type %s", port_info.port.c_str(), port_info.pcic_port, + port_info.type.c_str()); // Derive data_stream_type from PortInfo - if (port_type == "3D") + if (port_info.type == "3D") { data_stream_type = buffer_id_utils::data_stream_type::tof_3d; RCLCPP_INFO(logger_, "Data stream type is tof_3d."); } - else if (port_type == "2D") + else if (port_info.type == "2D") { data_stream_type = buffer_id_utils::data_stream_type::rgb_2d; RCLCPP_INFO(logger_, "Data stream type is rgb_2d."); @@ -1042,75 +466,12 @@ buffer_id_utils::data_stream_type CameraNode::stream_type_from_port_info(const s else { data_stream_type = buffer_id_utils::data_stream_type::tof_3d; - RCLCPP_ERROR(logger_, "Unknown data stream type '%s'. Defaulting to tof_3d.", port_type.c_str()); + RCLCPP_ERROR(logger_, "Unknown data stream type '%s'. Defaulting to tof_3d.", port_info.type.c_str()); } return data_stream_type; } -void CameraNode::publish_optical_link_transform() -{ - if (!tf_optical_link_publish_transform_) - { - // TF publication deactivated via parameter - return; - } - - tf2::Quaternion q; - q.setRPY(tf_optical_link_transform_[3], tf_optical_link_transform_[4], tf_optical_link_transform_[5]); - - geometry_msgs::msg::TransformStamped t; - t.header.stamp = this->get_clock()->now(); - t.header.frame_id = tf_mounting_link_frame_name_; - t.child_frame_id = tf_optical_link_frame_name_; - t.transform.translation.x = tf_optical_link_transform_[0]; - t.transform.translation.y = tf_optical_link_transform_[1]; - t.transform.translation.z = tf_optical_link_transform_[2]; - t.transform.rotation.x = q.x(); - t.transform.rotation.y = q.y(); - t.transform.rotation.z = q.z(); - t.transform.rotation.w = q.w(); - - tf_static_broadcaster_->sendTransform(t); -} - -void CameraNode::publish_cloud_link_transform_if_changed(const ExtrinsicsMsg& msg) -{ - if (!tf_cloud_link_publish_transform_) - { - // TF publication deactivated via parameter - return; - } - - tf2::Quaternion q; - q.setRPY(msg.rot_x, msg.rot_y, msg.rot_z); - - if ((cloud_link_transform_.header.frame_id == tf_optical_link_frame_name_) && - (cloud_link_transform_.child_frame_id == tf_cloud_link_frame_name_) && - (cloud_link_transform_.transform.translation.x == msg.tx) && - (cloud_link_transform_.transform.translation.y == msg.ty) && - (cloud_link_transform_.transform.translation.z == msg.tz) && - (cloud_link_transform_.transform.rotation.x == q.x()) && (cloud_link_transform_.transform.rotation.y == q.y()) && - (cloud_link_transform_.transform.rotation.z == q.z()) && (cloud_link_transform_.transform.rotation.w == q.w())) - { - // no change - return; - } - - cloud_link_transform_.header.stamp = this->get_clock()->now(); - cloud_link_transform_.header.frame_id = tf_optical_link_frame_name_; - cloud_link_transform_.child_frame_id = tf_cloud_link_frame_name_; - cloud_link_transform_.transform.translation.x = msg.tx; - cloud_link_transform_.transform.translation.y = msg.ty; - cloud_link_transform_.transform.translation.z = msg.tz; - cloud_link_transform_.transform.rotation.x = q.x(); - cloud_link_transform_.transform.rotation.y = q.y(); - cloud_link_transform_.transform.rotation.z = q.z(); - cloud_link_transform_.transform.rotation.w = q.w(); - - tf_static_broadcaster_->sendTransform(cloud_link_transform_); -} - void CameraNode::error_callback(const ifm3d::Error& error) { RCLCPP_ERROR(logger_, "Error received from ifm3d: %s", error.what()); @@ -1119,76 +480,15 @@ void CameraNode::error_callback(const ifm3d::Error& error) void CameraNode::async_error_callback(int i, const std::string& s) { - if (this->diag_mode_=="async"){ - RCLCPP_ERROR(logger_, "AsyncError received from ifm3d: %d %s", i, s.c_str()); - DiagnosticArrayMsg msg; - msg.header.stamp = get_clock()->now(); - msg.status.push_back(create_diagnostic_status(diagnostic_msgs::msg::DiagnosticStatus::ERROR, s)); - diagnostic_publisher_->publish(msg); - } + this->diag_module_->handle_error(i, s); + RCLCPP_DEBUG(this->logger_, "Async error callback done."); } void CameraNode::async_notification_callback(const std::string& s1, const std::string& s2) { - RCLCPP_INFO(logger_, "AsyncNotification received from ifm3d: %s | %s", s1.c_str(), s2.c_str()); - DiagnosticArrayMsg msg; - msg.header.stamp = get_clock()->now(); - msg.status.push_back(create_diagnostic_status(diagnostic_msgs::msg::DiagnosticStatus::OK, s2)); - diagnostic_publisher_->publish(msg); + this->diag_module_->handle_notification(s1, s2); + RCLCPP_DEBUG(this->logger_, "Async notification callback done."); } - -DiagnosticStatusMsg CameraNode::create_diagnostic_status(const uint8_t level, const std::string& json_msg) -{ - DiagnosticStatusMsg msg; - msg.level = level; - msg.hardware_id = hardware_id_; - - ifm3d::json parsed_json; - - try - { - parsed_json = json::parse(json_msg); - } - catch (...) - { - RCLCPP_ERROR(logger_, "Invalid JSON received from callback with level %d", level); - return msg; - } - - if (parsed_json.empty()) - { - RCLCPP_WARN(logger_, "Empty JSON received from callback with level %d", level); - return msg; - } - - if (parsed_json.contains("name")) - { - msg.name = parsed_json["name"]; - } - - if (parsed_json.contains("description")) - { - msg.message = parsed_json["description"]; - } - - for (auto& it : parsed_json.items()) - { - try - { - diagnostic_msgs::msg::KeyValue obj; - obj.key = it.key(); - obj.value = it.value().dump(); - msg.values.push_back(obj); - } - catch (const std::exception& e) - { - RCLCPP_WARN(logger_, "Couldn't parse entry of diagnostics status %s: %s", msg.name, e.what()); - } - } - - return msg; -} - } // namespace ifm3d_ros2 #include diff --git a/src/lib/camera_tf_publisher.cpp b/src/lib/camera_tf_publisher.cpp new file mode 100644 index 0000000..c5d7bb9 --- /dev/null +++ b/src/lib/camera_tf_publisher.cpp @@ -0,0 +1,223 @@ +#include + +#include + +namespace ifm3d_ros2 +{ + +CameraTfPublisher::CameraTfPublisher(rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr, ifm3d::O3R::Ptr o3r_ptr, + std::string port, std::string base_frame_name, std::string mounting_frame_name, + std::string optical_frame_name) + : tf_base_link_frame_name_(base_frame_name) + , tf_mounting_link_frame_name_(mounting_frame_name) + , tf_optical_link_frame_name_(optical_frame_name) + , tf_publish_mounting_to_optical_(true) + , tf_publish_base_to_mounting_(true) + , node_ptr_(node_ptr) + , o3r_ptr_(o3r_ptr) + , port_(port) +{ + tf_static_broadcaster_ = std::make_shared(this->node_ptr_); +} + +CameraTfPublisher::CameraTfPublisher(rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr, ifm3d::O3R::Ptr o3r_ptr, + std::string port) + : CameraTfPublisher(node_ptr, o3r_ptr, port, "", "", "") +{ +} + +bool CameraTfPublisher::update_and_publish_tf_if_changed( + const geometry_msgs::msg::TransformStamped& new_tf_base_to_optical) +{ + bool published_tf = false; + + if (transform_identical(new_tf_base_to_optical, tf_base_to_optical_)) + { + // No change in the TFs + return published_tf; + } + + // Update the TF2 on first run (tf_base_to_optical_ is not initialized) and on changes + tf_base_to_optical_ = new_tf_base_to_optical; + + const geometry_msgs::msg::TransformStamped new_tf_base_to_mounting = + read_tf_base_to_mounting_from_device_config(new_tf_base_to_optical.header.stamp); + + const geometry_msgs::msg::TransformStamped new_tf_mounting_to_optical = + get_tf_mounting_to_optical(new_tf_base_to_optical.header.stamp, new_tf_base_to_optical, new_tf_base_to_mounting); + + if (!transform_identical(new_tf_base_to_mounting, tf_base_to_mounting_)) + { + tf_base_to_mounting_ = new_tf_base_to_mounting; + + if (tf_publish_base_to_mounting_) + { + tf_static_broadcaster_->sendTransform(tf_base_to_mounting_); + published_tf = true; + RCLCPP_DEBUG(node_ptr_->get_logger(), "New Transform published:\n%s", + tf_to_string(new_tf_base_to_mounting).c_str()); + } + } + + if (!transform_identical(new_tf_mounting_to_optical, tf_mounting_to_optical_)) + { + tf_mounting_to_optical_ = new_tf_mounting_to_optical; + + if (tf_publish_mounting_to_optical_) + { + tf_static_broadcaster_->sendTransform(tf_mounting_to_optical_); + published_tf = true; + RCLCPP_DEBUG(node_ptr_->get_logger(), "New Transform published:\n%s", + tf_to_string(tf_mounting_to_optical_).c_str()); + } + } + + return published_tf; +} + +geometry_msgs::msg::TransformStamped +CameraTfPublisher::read_tf_base_to_mounting_from_device_config(builtin_interfaces::msg::Time stamp) +{ + // TODO use try-catch in case of json errors + + std::string json_string = "/ports/" + port_ + "/processing/extrinsicHeadToUser"; + ifm3d::json::json_pointer j_pointer(json_string); + auto config = o3r_ptr_->Get({ json_string })[j_pointer]; + + // Euler angles: + // ifm3d provides roll, pitch, yaw angles for intrisic rotations + // while tf2 expects them for extrinsic rotation. + // Therefore, a tf2 quaternion needs to be created from 3 separate rotations + tf2::Quaternion q_roll, q_pitch, q_yaw, q_combined; + q_roll.setRPY(config["rotX"], 0.0, 0.0); + q_pitch.setRPY(0.0, config["rotY"], 0.0); + q_yaw.setRPY(0.0, 0.0, config["rotZ"]); + q_combined = q_roll * q_pitch * q_yaw; + + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = stamp; + tf.header.frame_id = tf_base_link_frame_name_; + tf.child_frame_id = tf_mounting_link_frame_name_; + + tf.transform.translation.x = config["transX"]; + tf.transform.translation.y = config["transY"]; + tf.transform.translation.z = config["transZ"]; + tf.transform.rotation.x = q_combined.x(); + tf.transform.rotation.y = q_combined.y(); + tf.transform.rotation.z = q_combined.z(); + tf.transform.rotation.w = q_combined.w(); + + return tf; +} + +geometry_msgs::msg::TransformStamped CameraTfPublisher::get_tf_mounting_to_optical( + builtin_interfaces::msg::Time stamp, geometry_msgs::msg::TransformStamped tf_base_to_optical, + geometry_msgs::msg::TransformStamped tf_base_to_mounting) +{ + const tf2::Vector3 vector_mounting_to_optical_in_base( + tf_base_to_optical.transform.translation.x - tf_base_to_mounting.transform.translation.x, + tf_base_to_optical.transform.translation.y - tf_base_to_mounting.transform.translation.y, + tf_base_to_optical.transform.translation.z - tf_base_to_mounting.transform.translation.z); + + const tf2::Quaternion quad_base_to_mounting( + tf_base_to_mounting.transform.rotation.x, tf_base_to_mounting.transform.rotation.y, + tf_base_to_mounting.transform.rotation.z, tf_base_to_mounting.transform.rotation.w); + + const tf2::Quaternion quad_base_to_optical( + tf_base_to_optical.transform.rotation.x, tf_base_to_optical.transform.rotation.y, + tf_base_to_optical.transform.rotation.z, tf_base_to_optical.transform.rotation.w); + + // TODO check rotation here! + tf2::Vector3 vector_mounting_to_optical_in_mounting = + tf2::quatRotate(quad_base_to_mounting.inverse(), vector_mounting_to_optical_in_base); + + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = stamp; + tf.header.frame_id = tf_mounting_link_frame_name_; + tf.child_frame_id = tf_optical_link_frame_name_; + tf.transform.translation.x = vector_mounting_to_optical_in_mounting.x(); + tf.transform.translation.y = vector_mounting_to_optical_in_mounting.y(); + tf.transform.translation.z = vector_mounting_to_optical_in_mounting.z(); + + const tf2::Quaternion quad_mounting_to_optical(quad_base_to_optical * quad_base_to_mounting.inverse()); + tf.transform.rotation.x = quad_mounting_to_optical.x(); + tf.transform.rotation.y = quad_mounting_to_optical.y(); + tf.transform.rotation.z = quad_mounting_to_optical.z(); + tf.transform.rotation.w = quad_mounting_to_optical.w(); + + return tf; +} + +geometry_msgs::msg::TransformStamped CameraTfPublisher ::calculate_tf_base_to_optical( + builtin_interfaces::msg::Time stamp, geometry_msgs::msg::TransformStamped tf_base_to_mounting, + geometry_msgs::msg::TransformStamped tf_mounting_to_optical) +{ + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = stamp; + tf.header.frame_id = tf_base_link_frame_name_; + tf.child_frame_id = tf_optical_link_frame_name_; + + const tf2::Vector3 vector_base_to_mounting_in_base(tf_base_to_mounting.transform.translation.x, + tf_base_to_mounting.transform.translation.y, + tf_base_to_mounting.transform.translation.z); + const tf2::Vector3 vector_mounting_to_optical_in_mounting(tf_mounting_to_optical.transform.translation.x, + tf_mounting_to_optical.transform.translation.y, + tf_mounting_to_optical.transform.translation.z); + + const tf2::Quaternion quad_base_to_mounting( + tf_base_to_mounting.transform.rotation.x, tf_base_to_mounting.transform.rotation.y, + tf_base_to_mounting.transform.rotation.z, tf_base_to_mounting.transform.rotation.w); + + const tf2::Quaternion quad_mounting_to_optical( + tf_mounting_to_optical.transform.rotation.x, tf_mounting_to_optical.transform.rotation.y, + tf_mounting_to_optical.transform.rotation.z, tf_mounting_to_optical.transform.rotation.w); + + const tf2::Vector3 vector_mounting_to_optical_in_base = + tf2::quatRotate(quad_base_to_mounting.inverse(), vector_mounting_to_optical_in_mounting); + + const tf2::Vector3 vector_base_to_optical_in_base = + vector_base_to_mounting_in_base + vector_mounting_to_optical_in_base; + + const tf2::Quaternion quad_base_to_optical = quad_base_to_mounting * quad_mounting_to_optical; + + tf.transform.translation.x = vector_base_to_optical_in_base.x(); + tf.transform.translation.y = vector_base_to_optical_in_base.y(); + tf.transform.translation.z = vector_base_to_optical_in_base.z(); + tf.transform.rotation.x = quad_base_to_optical.x(); + tf.transform.rotation.y = quad_base_to_optical.y(); + tf.transform.rotation.z = quad_base_to_optical.z(); + tf.transform.rotation.w = quad_base_to_optical.w(); + + return tf; +} + +bool CameraTfPublisher::transform_identical(geometry_msgs::msg::TransformStamped tf1, + geometry_msgs::msg::TransformStamped tf2) +{ + return (tf1.child_frame_id == tf2.child_frame_id) && (tf1.header.frame_id == tf2.header.frame_id) && + (tf1.transform == tf2.transform); +} + +std::string CameraTfPublisher::tf_to_string(geometry_msgs::msg::TransformStamped tf) +{ + std::stringstream ss(""); + + ss << "frame_id=" << tf.header.frame_id << "\n"; + ss << "child_id=" << tf.child_frame_id << "\n"; + ss << "trans_x= " << tf.transform.translation.x << "\n"; + ss << "trans_y= " << tf.transform.translation.y << "\n"; + ss << "trans_z= " << tf.transform.translation.z << "\n"; + ss << "rot_x= " << tf.transform.rotation.x << "\n"; + ss << "rot_y= " << tf.transform.rotation.y << "\n"; + ss << "rot_z= " << tf.transform.rotation.z << "\n"; + ss << "rot_w= " << tf.transform.rotation.w << "\n"; + + tf2::Quaternion quad(tf.transform.rotation.x, tf.transform.rotation.y, tf.transform.rotation.z, + tf.transform.rotation.w); + + ss << "angle= " << quad.getAngle() << "\n"; + + return ss.str(); +} + +} // namespace ifm3d_ros2 \ No newline at end of file diff --git a/src/lib/diag_module.cpp b/src/lib/diag_module.cpp new file mode 100644 index 0000000..07a6634 --- /dev/null +++ b/src/lib/diag_module.cpp @@ -0,0 +1,207 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#include +#include +#include +#include + +#include +#include + +using json = ifm3d::json; +using namespace std::chrono_literals; +namespace ifm3d_ros2 +{ +DiagModule::DiagModule(rclcpp::Logger logger, rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr, + std::shared_ptr o3r) + : FunctionModule(logger), o3r_(o3r), node_ptr_(node_ptr) +{ + RCLCPP_DEBUG(logger_, "DiagModule contructor called."); + hardware_id_ = std::string(node_ptr_->get_namespace()) + "/" + std::string(get_name()); + RCLCPP_INFO(logger_, "hardware_id: %s", hardware_id_.c_str()); +} + +DiagModule::DiagnosticStatusMsg DiagModule::create_diagnostic_status(const uint8_t level, ifm3d::json parsed_json) +{ + DiagnosticStatusMsg msg; + msg.level = level; + + if (parsed_json.empty()) + { + RCLCPP_WARN(logger_, "Empty JSON received from callback with level %d", level); + return msg; + } + if (parsed_json.contains("source")) + { + msg.hardware_id = parsed_json["source"]; + } + if (parsed_json.contains("name")) + { + msg.name = parsed_json["name"]; + } + + if (parsed_json.contains("description")) + { + msg.message = parsed_json["description"]; + } + // TODO: should the KeyValue object be the diagnostic message id instead? + for (auto& it : parsed_json.items()) + { + try + { + diagnostic_msgs::msg::KeyValue obj; + obj.key = it.key(); + obj.value = it.value().dump(); + msg.values.push_back(obj); + } + catch (const std::exception& e) + { + RCLCPP_WARN(logger_, "Couldn't parse entry of diagnostics status %s: %s", msg.name.c_str(), e.what()); + } + } + return msg; +} + +DiagModule::DiagnosticArrayMsg DiagModule::create_diagnostic_message(const uint8_t level, const std::string& json_msg) +{ + ifm3d::json parsed_json; + DiagnosticArrayMsg diag_msg; + + try + { + parsed_json = json::parse(json_msg); + } + catch (...) + { + RCLCPP_ERROR(logger_, "Invalid JSON received from callback with level %d", level); + } + try + { + // The diagnostic message is formatted differently depending on whether + // we receive it through the asynchronous or synchronous method. + diag_msg.header.stamp = rclcpp::Time(parsed_json["timestamp"]); + } + catch (...) + { + try + { + diag_msg.header.stamp = rclcpp::Time(parsed_json["stats"]["lastActivated"]["timestamp"]); + } + catch (const std::exception& e) + { + RCLCPP_ERROR(logger_, "Invalid timestamp received from callback with level %d", level); + } + } + + diag_msg.status.push_back(create_diagnostic_status(level, parsed_json)); + + return diag_msg; +} + +void DiagModule::periodic_diag_callback() +{ + // TODO: do we really need this shared mutex here? + // std::lock_guard lock(this->gil_); + try + { + ifm3d::json diagnostic_json = this->o3r_->GetDiagnostic(); + RCLCPP_DEBUG(logger_, "Diagnostics: %s", diagnostic_json.dump().c_str()); + + auto events = diagnostic_json["events"]; + for (auto event : events) + { + diagnostic_publisher_->publish( + create_diagnostic_message(diagnostic_msgs::msg::DiagnosticStatus::OK, event.dump())); + } + } + catch (const ifm3d::Error& ex) + { + RCLCPP_INFO(logger_, "ifm3d error while trying to get the diagnostic: %s", ex.what()); + } + catch (...) + { + RCLCPP_INFO(logger_, "Unknown error while trying to get the diagnostic"); + } +} + +void DiagModule::handle_error(int i, const std::string& s) +{ + RCLCPP_ERROR(logger_, "AsyncError received from ifm3d: %d %s", i, s.c_str()); + DiagnosticArrayMsg msg = create_diagnostic_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, s); + diagnostic_publisher_->publish(msg); + RCLCPP_DEBUG(this->logger_, "Published the async diagnostic message."); +} + +void DiagModule::handle_notification(const std::string& s1, const std::string& s2) +{ + RCLCPP_INFO(logger_, "AsyncNotification received from ifm3d: %s | %s", s1.c_str(), s2.c_str()); + DiagnosticArrayMsg msg = create_diagnostic_message(diagnostic_msgs::msg::DiagnosticStatus::OK, s2); + diagnostic_publisher_->publish(msg); + RCLCPP_DEBUG(this->logger_, "Published the async notification."); +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn DiagModule::on_configure(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_DEBUG(logger_, "DiagModule: on_configure called"); + (void)previous_state; + + const auto qos = ifm3d_ros2::LowLatencyQoS(); + + this->diagnostic_publisher_ = node_ptr_->create_publisher("/diagnostics", qos); + // Timer for periodic publication of the full list of diagnostic messages + this->diagnostic_timer_ = this->node_ptr_->create_wall_timer(1s, [this]() -> void { periodic_diag_callback(); }); + this->diagnostic_timer_->cancel(); // Deactivate timer, manage activity via lifecycle + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn DiagModule::on_cleanup(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_DEBUG(logger_, "DiagModule: on_cleanup called"); + (void)previous_state; + + diagnostic_publisher_.reset(); + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn DiagModule::on_shutdown(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_DEBUG(logger_, "DiagModule: on_shutdown called"); + (void)previous_state; + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn DiagModule::on_activate(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_DEBUG(logger_, "DiagModule: on_activate called"); + (void)previous_state; + + this->diagnostic_publisher_->on_activate(); + this->diagnostic_timer_->reset(); + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn DiagModule::on_deactivate(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_DEBUG(logger_, "DiagModule: on_deactivate called"); + (void)previous_state; + + this->diagnostic_publisher_->on_deactivate(); + this->diagnostic_timer_->cancel(); + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn DiagModule::on_error(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_DEBUG(logger_, "DiagModule: on_error called"); + (void)previous_state; + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +} // namespace ifm3d_ros2 \ No newline at end of file diff --git a/src/lib/function_module.cpp b/src/lib/function_module.cpp new file mode 100644 index 0000000..ebb89f2 --- /dev/null +++ b/src/lib/function_module.cpp @@ -0,0 +1,22 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#include + +namespace ifm3d_ros2 +{ +FunctionModule::FunctionModule(const rclcpp::Logger& logger) : logger_(logger) +{ + RCLCPP_INFO(logger_, "FunctionModule contructor called."); +} + +void FunctionModule::handle_frame(ifm3d::Frame::Ptr frame) +{ + (void)frame; + RCLCPP_INFO(logger_, "FunctionModule: handle_frame called, implementation missing in derived class."); +} + +} // namespace ifm3d_ros2 \ No newline at end of file diff --git a/src/lib/ods_module.cpp b/src/lib/ods_module.cpp new file mode 100644 index 0000000..7791496 --- /dev/null +++ b/src/lib/ods_module.cpp @@ -0,0 +1,211 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace ifm3d_ros2 +{ +OdsModule::OdsModule(rclcpp::Logger logger, rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr) + : FunctionModule(logger), node_ptr_(node_ptr), frame_id_("ifm_base_link") +{ + RCLCPP_INFO(logger_, "OdsModule contructor called."); + + RCLCPP_DEBUG(logger_, "Declaring parameter..."); + frame_id_descriptor_.name = "ods.frame_id"; + frame_id_descriptor_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + frame_id_descriptor_.description = "Frame_id field used for grid and zones messages (Default='ifm_base_link')."; + node_ptr_->declare_parameter(frame_id_descriptor_.name, frame_id_, frame_id_descriptor_); + + publish_occupancy_grid_descriptor_.name = "ods.publish_occupancy_grid"; + publish_occupancy_grid_descriptor_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + publish_occupancy_grid_descriptor_.description = "Set module to publish nav_msgs/OccupancyGrid (Default='True')."; + node_ptr_->declare_parameter(publish_occupancy_grid_descriptor_.name, true, publish_occupancy_grid_descriptor_); + + publish_costmap_descriptor_.name = "ods.publish_costmap"; + publish_costmap_descriptor_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + publish_costmap_descriptor_.description = "Set module to publish nav3_msgs/Costmap (Default='False')."; + node_ptr_->declare_parameter(publish_costmap_descriptor_.name, false, publish_costmap_descriptor_); +} + +nav_msgs::msg::OccupancyGrid OdsModule::extract_ros_occupancy_grid(ifm3d::Frame::Ptr frame) +{ + RCLCPP_DEBUG(logger_, "Handling ods occupancy grid as nav_msgs/OccupancyGrid"); + if (!frame->HasBuffer(ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID)) + { + RCLCPP_INFO(logger_, "OdsModule: No ods occupancy grid in frame"); + } + auto header = std_msgs::msg::Header(); + header.frame_id = frame_id_; + return ifm3d_ros2::ifm3d_to_ros_occupancy_grid(frame->GetBuffer(ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID), header, + logger_); +} + +nav2_msgs::msg::Costmap OdsModule::extract_ros_costmap(ifm3d::Frame::Ptr frame) +{ + RCLCPP_DEBUG(logger_, "Handling ods occupancy grid as nav2_msgs/Costmap"); + if (!frame->HasBuffer(ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID)) + { + RCLCPP_INFO(logger_, "OdsModule: No ods occupancy grid in frame"); + } + auto header = std_msgs::msg::Header(); + header.frame_id = frame_id_; + return ifm3d_ros2::ifm3d_to_ros_costmap(frame->GetBuffer(ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID), header, logger_); +} + +ifm3d_ros2::msg::Zones OdsModule::extract_zones(ifm3d::Frame::Ptr frame) +{ + RCLCPP_DEBUG(logger_, "Handling zones"); + if (!frame->HasBuffer(ifm3d::buffer_id::O3R_ODS_INFO)) + { + RCLCPP_INFO(logger_, "OdsModule: No zones in frame"); + } + RCLCPP_DEBUG(logger_, "Deserializing zones data"); + auto zones_data = ifm3d::ODSInfoV1::Deserialize(frame->GetBuffer(ifm3d::buffer_id::O3R_ODS_INFO)); + + RCLCPP_DEBUG(logger_, "Filling the ROS message with zones data"); + ifm3d_ros2::msg::Zones zones_msg; + // Define the header + zones_msg.header = std_msgs::msg::Header(); + zones_msg.header.frame_id = frame_id_; + zones_msg.header.stamp = rclcpp::Time(zones_data.timestamp_ns); + + zones_msg.zone_config_id = zones_data.zone_config_id; + zones_msg.zone_occupied = zones_data.zone_occupied; + + RCLCPP_DEBUG(logger_, "Zones messages ready"); + return zones_msg; +} + +void OdsModule::handle_frame(ifm3d::Frame::Ptr frame) +{ + RCLCPP_DEBUG(logger_, "Handle frame"); + + RCLCPP_DEBUG(logger_, "Creating ods zones message."); + ZonesMsg zones_msg; + zones_msg = this->extract_zones(frame); + zones_publisher_->publish(zones_msg); + + if (publish_occupancy_grid_) + { + RCLCPP_DEBUG(logger_, "Creating occupancy grid message."); + OccupancyGridMsg grid_msg; + grid_msg = this->extract_ros_occupancy_grid(frame); + ros_occupancy_grid_publisher_->publish(grid_msg); + } + + if (publish_costmap_) + { + RCLCPP_DEBUG(logger_, "Creating costmap message."); + CostmapMsg costmap_msg; + costmap_msg = this->extract_ros_costmap(frame); + ros_costmap_publisher_->publish(costmap_msg); + } + + RCLCPP_DEBUG(logger_, "Finished publishing ods messages."); +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn OdsModule::on_configure(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "OdsModule: on_configure called"); + (void)previous_state; + + node_ptr_->get_parameter(frame_id_descriptor_.name, frame_id_); + node_ptr_->get_parameter(publish_occupancy_grid_descriptor_.name, publish_occupancy_grid_); + node_ptr_->get_parameter(publish_costmap_descriptor_.name, publish_costmap_); + RCLCPP_INFO(this->logger_, "Parameter %s set to '%s'", frame_id_descriptor_.name.c_str(), frame_id_.c_str()); + RCLCPP_INFO(this->logger_, "Parameter %s set to '%s'", publish_occupancy_grid_descriptor_.name.c_str(), + publish_occupancy_grid_ ? "true" : "false"); + RCLCPP_INFO(this->logger_, "Parameter %s set to '%s'", publish_costmap_descriptor_.name.c_str(), + publish_costmap_ ? "true" : "false"); + + const auto qos = ifm3d_ros2::LowLatencyQoS(); + + if (publish_occupancy_grid_) + { + ros_occupancy_grid_publisher_ = node_ptr_->create_publisher( + "~/" + buffer_id_utils::topic_name_map[ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID], qos); + } + if (publish_costmap_) + { + ros_costmap_publisher_ = node_ptr_->create_publisher( + "~/" + buffer_id_utils::topic_name_map[ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID] + "_costmap", qos); + } + zones_publisher_ = node_ptr_->create_publisher( + "~/" + buffer_id_utils::topic_name_map[ifm3d::buffer_id::O3R_ODS_INFO], qos); + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn OdsModule::on_cleanup(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "OdsModule: on_cleanup called"); + (void)previous_state; + + ros_occupancy_grid_publisher_.reset(); + ros_costmap_publisher_.reset(); + zones_publisher_.reset(); + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn OdsModule::on_shutdown(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "OdsModule: on_shutdown called"); + (void)previous_state; + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn OdsModule::on_activate(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "OdsModule: on_activate called"); + (void)previous_state; + + if (publish_occupancy_grid_) + { + this->ros_occupancy_grid_publisher_->on_activate(); + } + if (publish_costmap_) + { + this->ros_costmap_publisher_->on_activate(); + } + this->zones_publisher_->on_activate(); + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn OdsModule::on_deactivate(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "OdsModule: on_deactivate called"); + (void)previous_state; + + if (publish_occupancy_grid_) + { + this->ros_occupancy_grid_publisher_->on_deactivate(); + } + if (publish_costmap_) + { + this->ros_costmap_publisher_->on_deactivate(); + } + this->zones_publisher_->on_deactivate(); + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn OdsModule::on_error(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "OdsModule: on_error called"); + (void)previous_state; + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +} // namespace ifm3d_ros2 \ No newline at end of file diff --git a/src/lib/ods_node.cpp b/src/lib/ods_node.cpp new file mode 100644 index 0000000..6945cc2 --- /dev/null +++ b/src/lib/ods_node.cpp @@ -0,0 +1,434 @@ +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using json = ifm3d::json; +using namespace std::chrono_literals; + +namespace ifm3d_ros2 +{ +namespace +{ +} // namespace + +OdsNode::OdsNode(const rclcpp::NodeOptions& opts) : OdsNode::OdsNode("ods_node", opts) +{ +} + +OdsNode::OdsNode(const std::string& node_name, const rclcpp::NodeOptions& opts) + : rclcpp_lifecycle::LifecycleNode(node_name, "", opts), logger_(this->get_logger()), width_(0), height_(0) +{ + // unbuffered I/O to stdout (so we can see our log messages) + std::setvbuf(stdout, nullptr, _IONBF, BUFSIZ); + RCLCPP_INFO(this->logger_, "namespace: %s", this->get_namespace()); + RCLCPP_INFO(this->logger_, "node name: %s", this->get_name()); + RCLCPP_INFO(this->logger_, "middleware: %s", rmw_get_implementation_identifier()); + + // declare our parameters and default values -- parameters defined in + // the passed in `opts` (via __params:=/path/to/params.yaml on cmd line) + // will override our default values specified. + RCLCPP_INFO(this->logger_, "Declaring parameters..."); + this->init_params(); + RCLCPP_INFO(this->logger_, "After the parameters declaration"); + + this->gil_ = std::make_shared(); + + RCLCPP_INFO(this->logger_, "node created, waiting for `configure()`..."); +} + +OdsNode::~OdsNode() +{ + RCLCPP_INFO(this->logger_, "Dtor called."); +} + +TC_RETVAL OdsNode::on_configure(const rclcpp_lifecycle::State& prev_state) +{ + RCLCPP_INFO(this->logger_, "on_configure(): %s -> %s", prev_state.label().c_str(), + this->get_current_state().label().c_str()); + + // + // parse params and initialize instance vars + // + RCLCPP_INFO(this->logger_, "Parsing parameters..."); + parse_params(); + RCLCPP_INFO(this->logger_, "Parameters parsed."); + + // Add parameter subscriber, if none is active + if (!param_subscriber_) + { + RCLCPP_INFO(logger_, "Adding callbacks to handle parameter changes at runtime..."); + set_parameter_event_callbacks(); + RCLCPP_INFO(this->logger_, "Callbacks set."); + } + + // + // We need a global lock on all the ifm3d core data structures + // + std::lock_guard lock(*this->gil_); + + // + // Initialize the camera interface + // + RCLCPP_INFO(this->logger_, "Initializing Device"); + this->o3r_ = std::make_shared(this->ip_); + RCLCPP_INFO(this->logger_, "Initializing FrameGrabber for data"); + this->fg_ = std::make_shared(this->o3r_, this->pcic_port_); + RCLCPP_DEBUG(this->logger_, "Initializing FrameGrabber for diagnostics"); + this->fg_diag_ = std::make_shared(this->o3r_, this->o3r_->Port("diagnostics").pcic_port); + RCLCPP_DEBUG(this->logger_, "Find out which data stream we are handling"); + + // If a configuration file is provided, configure the device. + if (this->config_file_!=""){ + std::ifstream file(this->config_file_); + if (!file.is_open()) { + throw std::runtime_error("Could not open config file: " + this->config_file_); + } + std::stringstream buffer; + buffer << file.rdbuf(); + RCLCPP_INFO(this->logger_, "Setting configuration: %s", buffer.str().c_str()); + ifm3d::json config_json = json::parse(buffer.str()) ; + this->o3r_->Set(config_json); + } + + // Get all the necessary info for the port. + for (auto port : this->o3r_->Ports()) + { + if (port.pcic_port == this->pcic_port_) + { + this->port_info_ = port; + } + } + + // + // Initialize ODS Module + // + RCLCPP_INFO(this->logger_, "Creating OdsModule..."); + this->ods_module_ = std::make_shared(this->get_logger(), shared_from_this()); + RCLCPP_INFO(this->logger_, "OdsModule created."); + // + // Initialize diagnostic Module + // + RCLCPP_INFO(this->logger_, "Creating DiagModule..."); + this->diag_module_ = std::make_shared(this->get_logger(), shared_from_this(), this->o3r_); + RCLCPP_INFO(this->logger_, "DiagModule created."); + + // + // Create a list of all the modules to reduce duplicate code + // + this->modules_.push_back(this->ods_module_); + this->modules_.push_back(this->diag_module_); + + // Transition function modules + for (auto& module : this->modules_) + { + auto retval = module->on_configure(prev_state); + if (retval != TC_RETVAL::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Module %s transition did not succeed.", module->get_name().c_str()); + // TODO de-transition previous modules + return retval; + } + } + + // Initialize the services using the BaseServices class + RCLCPP_INFO(this->logger_, "Creating BaseServices..."); + this->base_services_ = + std::make_shared(this->get_logger(), shared_from_this(), this->o3r_, this->port_info_, this->gil_); + RCLCPP_INFO(this->logger_, "BaseServices created."); + + RCLCPP_INFO(this->logger_, "Configuration complete."); + return TC_RETVAL::SUCCESS; +} + +TC_RETVAL OdsNode::on_activate(const rclcpp_lifecycle::State& prev_state) +{ + RCLCPP_INFO(this->logger_, "on_activate(): %s -> %s", prev_state.label().c_str(), + this->get_current_state().label().c_str()); + + // Register Callbacks to handle new frames and print errors + RCLCPP_DEBUG(this->logger_, "Registering the callbacks"); + this->fg_->OnNewFrame(std::bind(&OdsNode::frame_callback, this, std::placeholders::_1)); + this->fg_->OnError(std::bind(&OdsNode::error_callback, this, std::placeholders::_1)); + + this->fg_diag_->OnAsyncError( + std::bind(&OdsNode::async_error_callback, this, std::placeholders::_1, std::placeholders::_2)); + this->fg_diag_->OnAsyncNotification( + std::bind(&OdsNode::async_notification_callback, this, std::placeholders::_1, std::placeholders::_2)); + RCLCPP_DEBUG(this->logger_, "Registered all the callbacks"); + + // The Framegrabber, needs a BufferList (a vector of std::variant) + RCLCPP_INFO(this->logger_, "Starting the Framegrabbers..."); + ifm3d::FrameGrabber::BufferList buffer_list; + buffer_list.push_back(ifm3d::buffer_id::O3R_ODS_INFO); + buffer_list.push_back(ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID); + + // Start framegrabbers and wait for the returned future + this->fg_->Start(buffer_list).wait(); + RCLCPP_DEBUG(this->logger_, "Data FrameGrabber started, frames should be streaming"); + + this->fg_diag_->Start({}).wait(); + RCLCPP_INFO(this->logger_, "Diagnostic monitoring active."); + + // Transition function modules + for (auto& module : this->modules_) + { + auto retval = module->on_activate(prev_state); + if (retval != TC_RETVAL::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Module %s transition did not succeed.", module->get_name().c_str()); + // TODO de-transition previous modules + return retval; + } + } + return TC_RETVAL::SUCCESS; +} + +TC_RETVAL OdsNode::on_deactivate(const rclcpp_lifecycle::State& prev_state) +{ + RCLCPP_INFO(this->logger_, "on_deactivate(): %s -> %s", prev_state.label().c_str(), + this->get_current_state().label().c_str()); + + RCLCPP_INFO(logger_, "Stopping FrameGrabber..."); + this->fg_->Stop().wait(); + + // Transition function modules + for (auto& module : this->modules_) + { + auto retval = module->on_deactivate(prev_state); + if (retval != TC_RETVAL::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Module %s transition did not succeed.", module->get_name().c_str()); + // TODO de-transition previous modules + return retval; + } + } + + return TC_RETVAL::SUCCESS; +} + +TC_RETVAL OdsNode::on_cleanup(const rclcpp_lifecycle::State& prev_state) +{ + // clean-up resources -- this will include our cam, fg, + RCLCPP_INFO(this->logger_, "on_cleanup(): %s -> %s", prev_state.label().c_str(), + this->get_current_state().label().c_str()); + + std::lock_guard lock(*this->gil_); + RCLCPP_INFO(this->logger_, "Resetting core ifm3d data structures..."); + + this->fg_.reset(); + this->o3r_.reset(); + + // Transition function modules + for (auto& module : this->modules_) + { + auto retval = module->on_cleanup(prev_state); + if (retval != TC_RETVAL::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Module %s transition did not succeed.", module->get_name().c_str()); + // TODO de-transition previous modules + return retval; + } + module.reset(); + } + // this->modules_.reset(); + + RCLCPP_INFO(this->logger_, "Node cleanup complete."); + + return TC_RETVAL::SUCCESS; +} + +TC_RETVAL OdsNode::on_shutdown(const rclcpp_lifecycle::State& prev_state) +{ + RCLCPP_INFO(this->logger_, "on_shutdown(): %s -> %s", prev_state.label().c_str(), + this->get_current_state().label().c_str()); + + // TODO: figure out how to properly shutdown modules + this->fg_->Stop(); + this->fg_diag_->Stop(); + + // Transition function modules + for (auto& module : this->modules_) + { + auto retval = module->on_shutdown(prev_state); + if (retval != TC_RETVAL::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Module %s transition did not succeed.", module->get_name().c_str()); + // TODO de-transition previous modules + return retval; + } + } + return TC_RETVAL::SUCCESS; +} + +TC_RETVAL OdsNode::on_error(const rclcpp_lifecycle::State& prev_state) +{ + RCLCPP_INFO(this->logger_, "on_error(): %s -> %s", prev_state.label().c_str(), + this->get_current_state().label().c_str()); + + std::lock_guard lock(*this->gil_); + RCLCPP_INFO(this->logger_, "Resetting core ifm3d data structures..."); + // this->im_.reset(); + this->fg_.reset(); + this->o3r_.reset(); + + // Transition function modules + // Transition function modules + for (auto& module : this->modules_) + { + auto retval = module->on_error(prev_state); + if (retval != TC_RETVAL::SUCCESS) + { + RCLCPP_ERROR(this->get_logger(), "Module %s transition did not succeed.", module->get_name().c_str()); + // TODO de-transition previous modules + return retval; + } + } + + RCLCPP_INFO(this->logger_, "Error processing complete."); + + return TC_RETVAL::SUCCESS; +} + +void OdsNode::init_params() // TODO cleanup params +{ + // Node name as string to set default frame names + const std::string node_name(this->get_name()); + + /* + * For all parameters in alphabetical order: + * - Define Descriptor + * - Declare Parameter + */ + + rcl_interfaces::msg::ParameterDescriptor ip_descriptor; + ip_descriptor.name = "ip"; + ip_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + ip_descriptor.description = "IP address of the camera"; + ip_descriptor.additional_constraints = "Should be an IPv4 address or resolvable name on your network"; + this->declare_parameter("ip", ifm3d::DEFAULT_IP, ip_descriptor); + + rcl_interfaces::msg::ParameterDescriptor pcic_port_descriptor; + pcic_port_descriptor.name = "pcic_port"; + pcic_port_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + pcic_port_descriptor.description = + " TCP port the on-image processing platform pcic server is listening on. Corresponds to the port the camera head " + "is connected to."; + this->declare_parameter("pcic_port", 51010, pcic_port_descriptor); + + rcl_interfaces::msg::ParameterDescriptor xmlrpc_port_descriptor; + xmlrpc_port_descriptor.name = "xmlrpc_port"; + xmlrpc_port_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + xmlrpc_port_descriptor.description = "TCP port the on-camera xmlrpc server is listening on"; + xmlrpc_port_descriptor.additional_constraints = "A valid TCP port 0 - 65535"; + rcl_interfaces::msg::IntegerRange xmlrpc_port_range; + xmlrpc_port_range.from_value = 0; + xmlrpc_port_range.to_value = 65535; + xmlrpc_port_range.step = 1; + xmlrpc_port_descriptor.integer_range.push_back(xmlrpc_port_range); + this->declare_parameter("xmlrpc_port", ifm3d::DEFAULT_XMLRPC_PORT, xmlrpc_port_descriptor); +} + +void OdsNode::parse_params() +{ + /* + * For all parameters in alphabetical order: + * - Read currently set parameter + * - Where applicable, parse read data into more useful data type + */ + this->get_parameter("config_file", this->config_file_); + RCLCPP_INFO(this->logger_, "Config file: %s", this->config_file_.c_str()); + + this->get_parameter("ip", this->ip_); + RCLCPP_INFO(this->logger_, "ip: %s", this->ip_.c_str()); + + this->get_parameter("pcic_port", this->pcic_port_); + RCLCPP_INFO(this->logger_, "pcic_port: %u", this->pcic_port_); + + this->get_parameter("xmlrpc_port", this->xmlrpc_port_); + RCLCPP_INFO(this->logger_, "xmlrpc_port: %u", this->xmlrpc_port_); +} + +void OdsNode::set_parameter_event_callbacks() +{ + // Create a parameter subscriber that can be used to monitor parameter changes + param_subscriber_ = std::make_shared(this); + + /* + * For all parameters in alphabetical order: + * - Create a callback as lambda to handle parameter change at runtime + * - Add lambda to parameter subscriber + */ + auto config_file_cb = [this](const rclcpp::Parameter& p) { + RCLCPP_WARN(logger_, "This new config_file will be used after CONFIGURE transition was called: '%s'", p.as_string().c_str()); + }; + registered_param_callbacks_["config_file"] = param_subscriber_->add_parameter_callback("config_file", config_file_cb); + + auto ip_cb = [this](const rclcpp::Parameter& p) { + RCLCPP_WARN(logger_, "This new ip will be used after CONFIGURE transition was called: '%s'", p.as_string().c_str()); + }; + registered_param_callbacks_["ip"] = param_subscriber_->add_parameter_callback("ip", ip_cb); + + auto pcic_port_cb = [this](const rclcpp::Parameter& p) { + RCLCPP_WARN(logger_, "This new pcic_port will be used after CONFIGURE transition was called: %ld", p.as_int()); + }; + registered_param_callbacks_["pcic_port"] = param_subscriber_->add_parameter_callback("pcic_port", pcic_port_cb); + + auto xmlrpc_port_cb = [this](const rclcpp::Parameter& p) { + this->xmlrpc_port_ = p.as_int(); + RCLCPP_INFO(logger_, "New xmlrpc_port: %d", this->xmlrpc_port_); + }; + registered_param_callbacks_["xmlrpc_port"] = param_subscriber_->add_parameter_callback("xmlrpc_port", xmlrpc_port_cb); +} + +void OdsNode::frame_callback(ifm3d::Frame::Ptr frame) +{ + this->ods_module_->handle_frame(frame); + + RCLCPP_DEBUG(this->logger_, "Frame callback done."); +} + +void OdsNode::error_callback(const ifm3d::Error& error) +{ + RCLCPP_ERROR(logger_, "Error received from ifm3d: %s", error.what()); + // TODO send diagnostic message +} + +void OdsNode::async_error_callback(int i, const std::string& s) +{ + this->diag_module_->handle_error(i, s); + RCLCPP_DEBUG(this->logger_, "Async error callback done."); +} + +void OdsNode::async_notification_callback(const std::string& s1, const std::string& s2) +{ + this->diag_module_->handle_notification(s1, s2); + RCLCPP_DEBUG(this->logger_, "Async notification callback done."); +} + +} // namespace ifm3d_ros2 + +#include + +// RCLCPP_COMPONENTS_REGISTER_NODE(ifm3d_ros2::OdsNode) diff --git a/src/lib/rgb_module.cpp b/src/lib/rgb_module.cpp new file mode 100644 index 0000000..6de3c8b --- /dev/null +++ b/src/lib/rgb_module.cpp @@ -0,0 +1,363 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#include +#include +#include +#include + +#include +#include +#include + +namespace ifm3d_ros2 +{ +RgbModule::RgbModule(rclcpp::Logger logger, rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr, + ifm3d::O3R::Ptr o3r_ptr, std::string port, uint32_t width, uint32_t height) + : FunctionModule(logger), node_ptr_(node_ptr), tf_publisher_(node_ptr, o3r_ptr, port), width_(width), height_(height) +{ + RCLCPP_INFO(logger_, "RgbModule contructor called."); + + RCLCPP_DEBUG(this->logger_, "Declaring parameters..."); + const std::string node_name(this->node_ptr_->get_name()); + const std::vector default_buffer_id_list{ + "JPEG_IMAGE", // + "RGB_INFO", // + }; + buffer_id_list_descriptor_.name = "buffer_id_list"; + buffer_id_list_descriptor_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; + buffer_id_list_descriptor_.description = "List of buffer_id strings denoting the wanted buffers."; + this->node_ptr_->declare_parameter(buffer_id_list_descriptor_.name, default_buffer_id_list, + buffer_id_list_descriptor_); + + tf_base_frame_name_descriptor_.name = "tf.base_frame_name"; + tf_base_frame_name_descriptor_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + tf_base_frame_name_descriptor_.description = "Name for the ifm base frame, defaults to ifm_base_link."; + this->node_ptr_->declare_parameter(tf_base_frame_name_descriptor_.name, "ifm_base_link", + tf_base_frame_name_descriptor_); + + tf_mounting_frame_name_descriptor_.name = "tf.mounting_frame_name"; + tf_mounting_frame_name_descriptor_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + tf_mounting_frame_name_descriptor_.description = + "Name for the mounting point frame, defaults to _mounting_link."; + this->node_ptr_->declare_parameter(tf_mounting_frame_name_descriptor_.name, node_name + "_mounting_link", + tf_mounting_frame_name_descriptor_); + + tf_optical_frame_name_descriptor_.name = "tf.optical_frame_name"; + tf_optical_frame_name_descriptor_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + tf_optical_frame_name_descriptor_.description = + "Name for the point optical frame, defaults to _optical_link."; + this->node_ptr_->declare_parameter(tf_optical_frame_name_descriptor_.name, node_name + "_optical_link", + tf_optical_frame_name_descriptor_); + + tf_publish_base_to_mounting_descriptor_.name = "tf.publish_base_to_mounting"; + tf_publish_base_to_mounting_descriptor_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + tf_publish_base_to_mounting_descriptor_.description = + "Whether the transform from the ifm base link to the mounting point should be published."; + this->node_ptr_->declare_parameter(tf_publish_base_to_mounting_descriptor_.name, true, + tf_publish_base_to_mounting_descriptor_); + + tf_publish_mounting_to_optical_descriptor_.name = "tf.publish_mounting_to_optical"; + tf_publish_mounting_to_optical_descriptor_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + tf_publish_mounting_to_optical_descriptor_.description = + "Whether the transform from the mounting point to the optical frame should be published."; + this->node_ptr_->declare_parameter(tf_publish_mounting_to_optical_descriptor_.name, true, + tf_publish_mounting_to_optical_descriptor_); + RCLCPP_DEBUG(this->logger_, "After the parameters declaration"); + + this->first_ = true; +} + +void RgbModule::handle_frame(ifm3d::Frame::Ptr frame) +{ + RCLCPP_DEBUG(logger_, "Handle RGB frame"); + using namespace buffer_id_utils; + + RCLCPP_INFO_ONCE(logger_, "Receiving Frames. Processing buffer for [%s]...", + buffer_id_utils::vector_to_string(this->buffer_id_list_).c_str()); + RCLCPP_DEBUG(logger_, "Received new Frame."); + + rclcpp::Time frame_ts = ifm3d_ros2::ifm3d_to_ros_time(frame->TimeStamps()[0]); + RCLCPP_DEBUG(logger_, "Frame timestamp: %f", frame_ts.seconds()); + + auto optical_header = std_msgs::msg::Header(); + optical_header.frame_id = tf_publisher_.tf_optical_link_frame_name_; + optical_header.stamp = frame_ts; + + RCLCPP_DEBUG(logger_, "RGB message headers created"); + + for (const ifm3d::buffer_id& id : this->buffer_id_list_) + { + // Helper for logging + auto& clk = *this->node_ptr_->get_clock(); + std::string id_string; + if (!buffer_id_utils::convert(id, id_string)){ + RCLCPP_ERROR(logger_, "Cannot convert the buffer id to a string."); + } + + const buffer_id_utils::message_type message_type = buffer_id_utils::message_type_map[id]; + RCLCPP_DEBUG(logger_, "Processing buffer_id %s", id_string.c_str()); + + if (!frame->HasBuffer(id)) + { + RCLCPP_WARN_THROTTLE(logger_, clk, 5000, + "Frame does not contain buffer %s. Is the correct camera head connected?", + id_string.c_str()); + } + + switch (message_type) + { + case buffer_id_utils::message_type::compressed_image: { + RCLCPP_DEBUG(logger_, "Processing compressed image message"); + auto buffer = frame->GetBuffer(id); + CompressedImageMsg compressed_image_msg = + ifm3d_ros2::ifm3d_to_ros_compressed_image(frame->GetBuffer(id), optical_header, "jpeg", logger_); + RCLCPP_DEBUG(logger_, "Ready to publish compressed image message"); + this->rgb_publisher_->publish(compressed_image_msg); + RCLCPP_DEBUG(logger_, "Published compressed image message"); + } + break; + case buffer_id_utils::message_type::rgb_info: { + RCLCPP_DEBUG(logger_, "Processing RGB info message"); + // Publish the ifm-type RGB info message + auto buffer = frame->GetBuffer(id); + RGBInfoMsg rgb_info_msg = ifm3d_ros2::ifm3d_to_rgb_info(buffer, optical_header, logger_); + rgb_info_publisher_->publish(rgb_info_msg); + + if (tf_publisher_.tf_publish_mounting_to_optical_ || tf_publisher_.tf_publish_base_to_mounting_) + { + geometry_msgs::msg::TransformStamped tf_base_to_optical; + if (ifm3d_ros2::ifm3d_rgb_info_to_optical_mount_link(buffer, tf_publisher_.tf_base_link_frame_name_, + tf_publisher_.tf_optical_link_frame_name_, logger_, + tf_base_to_optical)) + { + tf_publisher_.update_and_publish_tf_if_changed(tf_base_to_optical); + } + else + { + RCLCPP_ERROR(logger_, "Failed to derive transform from rgb_info, retrying..."); + } + } + this->width_ = 1280; // TODO: figure out how to read the width and height from JPEG image + this->height_ = 800; + // Also publish CameraInfo from RGBInfo + if (this->width_ == 0 || this->height_ == 0) + { + RCLCPP_WARN_THROTTLE(logger_, clk, 5000, "Needs at least one raw image buffer to parse CameraInfo!"); + } + else + { + CameraInfoMsg camera_info_msg; + + if (ifm3d_ros2::ifm3d_rgb_info_to_camera_info(buffer, optical_header, this->height_, this->width_, logger_, + camera_info_msg)) + { + RCLCPP_INFO_ONCE(logger_, "Parsing CameraInfo successfull."); + camera_info_publisher_->publish(camera_info_msg); + } + else + { + RCLCPP_ERROR(logger_, "Failed to retrieve camera_info from rgb_info buffer."); + }; + } + } + break; + default: + RCLCPP_ERROR_THROTTLE(logger_, clk, 5000, "Unknown message type for buffer_id %s. Can not publish.", + id_string.c_str()); + break; + } + } + RCLCPP_DEBUG(logger_, "Finished publishing rgb messages."); +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn RgbModule::on_configure(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "RgbModule: on_configure called"); + (void)previous_state; + + // + // parse params and initialize instance vars + // + RCLCPP_INFO(this->logger_, "Parsing parameters..."); + parse_params(); + RCLCPP_INFO(this->logger_, "Parameters parsed."); + + // Add parameter subscriber, if none is active + if (!param_subscriber_) + { + RCLCPP_INFO(logger_, "Adding callbacks to handle parameter changes at runtime..."); + set_parameter_event_callbacks(); + RCLCPP_INFO(this->logger_, "Callbacks set."); + } + + // Remove buffer_ids unfit for the given data type + this->buffer_id_list_ = + buffer_id_utils::buffer_ids_for_data_stream_type(this->buffer_id_list_, ifm3d_ros2::buffer_id_utils::data_stream_type::rgb_2d); + RCLCPP_INFO(logger_, "After removing buffer_ids unfit for the given data stream type, the final list is: [%s].", + buffer_id_utils::vector_to_string(this->buffer_id_list_).c_str()); + + std::vector ids_to_remove{}; + // Create correctly typed publishers for all given buffer_ids + for (const ifm3d::buffer_id& id : this->buffer_id_list_) + { + // Create Publishers in node namespace to make multi-camera setups easier + const std::string topic_name = "~/" + buffer_id_utils::topic_name_map[id]; + const auto qos = ifm3d_ros2::LowLatencyQoS(); + const buffer_id_utils::message_type message_type = buffer_id_utils::message_type_map[id]; + + switch (message_type) + { + case buffer_id_utils::message_type::compressed_image: + rgb_publisher_ = node_ptr_->create_publisher(topic_name, qos); + break; + case buffer_id_utils::message_type::rgb_info: + rgb_info_publisher_ = node_ptr_->create_publisher(topic_name, qos); + camera_info_publisher_ = node_ptr_->create_publisher("~/camera_info", qos); + break; + default: + std::string id_string; + buffer_id_utils::convert(id, id_string); + RCLCPP_ERROR(logger_, "Unknown message type for buffer_id %s. Will be removed from list...", id_string.c_str()); + ids_to_remove.push_back(id); + break; + } + } + RCLCPP_INFO(this->logger_, "Created publishers for all buffer_ids."); + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn RgbModule::on_cleanup(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "RgbModule: on_cleanup called"); + (void)previous_state; + + this->rgb_publisher_.reset(); + this->rgb_info_publisher_.reset(); + this->camera_info_publisher_.reset(); + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn RgbModule::on_shutdown(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "RgbModule: on_shutdown called"); + (void)previous_state; + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn RgbModule::on_activate(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "RgbModule: on_activate called"); + (void)previous_state; + + this->rgb_publisher_->on_activate(); + this->rgb_info_publisher_->on_activate(); + this->camera_info_publisher_->on_activate(); + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn RgbModule::on_deactivate(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "RgbModule: on_deactivate called"); + (void)previous_state; + + this->rgb_publisher_->on_deactivate(); + this->rgb_info_publisher_->on_deactivate(); + this->camera_info_publisher_->on_deactivate(); + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn RgbModule::on_error(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "RgbModule: on_error called"); + (void)previous_state; + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +void RgbModule::parse_params() +{ + std::vector buffer_id_strings; + this->node_ptr_->get_parameter(buffer_id_list_descriptor_.name, buffer_id_strings); + RCLCPP_INFO(this->logger_, "Reading %ld buffer_ids: [%s]", buffer_id_strings.size(), + buffer_id_utils::vector_to_string(buffer_id_strings).c_str()); + // Populate buffer_id_list_ from read strings + this->buffer_id_list_.clear(); + for (const std::string& string : buffer_id_strings) + { + ifm3d::buffer_id found_id; + if (buffer_id_utils::convert(string, found_id)) + { + this->buffer_id_list_.push_back(found_id); + } + else + { + RCLCPP_WARN(this->logger_, "Ignoring unknown buffer_id %s", string.c_str()); + } + } + RCLCPP_INFO(this->logger_, "Parsed %ld buffer_ids: %s", this->buffer_id_list_.size(), + buffer_id_utils::vector_to_string(this->buffer_id_list_).c_str()); + + this->node_ptr_->get_parameter(tf_base_frame_name_descriptor_.name, tf_publisher_.tf_base_link_frame_name_); + RCLCPP_INFO(this->logger_, "tf.base_link.frame_name: %s", tf_publisher_.tf_base_link_frame_name_.c_str()); + + this->node_ptr_->get_parameter(tf_mounting_frame_name_descriptor_.name, tf_publisher_.tf_mounting_link_frame_name_); + RCLCPP_INFO(this->logger_, "tf.mounting_link.frame_name: %s", tf_publisher_.tf_mounting_link_frame_name_.c_str()); + + this->node_ptr_->get_parameter(tf_optical_frame_name_descriptor_.name, tf_publisher_.tf_optical_link_frame_name_); + RCLCPP_INFO(this->logger_, "tf.optical_link.frame_name: %s", tf_publisher_.tf_optical_link_frame_name_.c_str()); + + this->node_ptr_->get_parameter(tf_publish_base_to_mounting_descriptor_.name, + tf_publisher_.tf_publish_base_to_mounting_); + + this->node_ptr_->get_parameter(tf_publish_mounting_to_optical_descriptor_.name, + tf_publisher_.tf_publish_mounting_to_optical_); + + RCLCPP_INFO(this->logger_, "tf.optical_link.publish_transform: %s", + tf_publisher_.tf_publish_mounting_to_optical_ ? "true" : "false"); +} + +void RgbModule::set_parameter_event_callbacks() +{ + // Create a parameter subscriber that can be used to monitor parameter changes + param_subscriber_ = std::make_shared(this->node_ptr_); + + auto buffer_id_list_cb = [this](const rclcpp::Parameter& p) { + RCLCPP_WARN(logger_, "This new buffer_id_list will be used after CONFIGURE transition was called: %s", + buffer_id_utils::vector_to_string(p.as_string_array()).c_str()); + }; + registered_param_callbacks_[buffer_id_list_descriptor_.name] = + param_subscriber_->add_parameter_callback(buffer_id_list_descriptor_.name, buffer_id_list_cb); + + auto tf_mounting_link_frame_name_cb = [this](const rclcpp::Parameter& p) { + tf_publisher_.tf_mounting_link_frame_name_ = p.as_string(); + RCLCPP_INFO(logger_, "New tf.mounting_link.frame_name: '%s'", tf_publisher_.tf_mounting_link_frame_name_.c_str()); + }; + registered_param_callbacks_[tf_mounting_frame_name_descriptor_.name] = param_subscriber_->add_parameter_callback( + tf_mounting_frame_name_descriptor_.name, tf_mounting_link_frame_name_cb); + + auto tf_optical_link_frame_name_cb = [this](const rclcpp::Parameter& p) { + tf_publisher_.tf_optical_link_frame_name_ = p.as_string(); + RCLCPP_INFO(logger_, "New tf.optical_link.frame_name: '%s'", tf_publisher_.tf_optical_link_frame_name_.c_str()); + }; + registered_param_callbacks_[tf_optical_frame_name_descriptor_.name] = + param_subscriber_->add_parameter_callback(tf_optical_frame_name_descriptor_.name, tf_optical_link_frame_name_cb); + + auto tf_optical_link_publish_transform_cb = [this](const rclcpp::Parameter& p) { + tf_publisher_.tf_publish_mounting_to_optical_ = p.as_bool(); + RCLCPP_INFO(logger_, "New tf.optical_link.publish_transform: %s", + tf_publisher_.tf_publish_mounting_to_optical_ ? "true" : "false"); + }; + registered_param_callbacks_[tf_publish_mounting_to_optical_descriptor_.name] = + param_subscriber_->add_parameter_callback(tf_publish_mounting_to_optical_descriptor_.name, + tf_optical_link_publish_transform_cb); +} + +} // namespace ifm3d_ros2 \ No newline at end of file diff --git a/src/lib/services.cpp b/src/lib/services.cpp new file mode 100644 index 0000000..9567daa --- /dev/null +++ b/src/lib/services.cpp @@ -0,0 +1,292 @@ +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ +#include +#include +#include + +#include "ifm3d_ros2/services.hpp" + +using json = ifm3d::json; + +namespace ifm3d_ros2 +{ +BaseServices::BaseServices(rclcpp::Logger logger, rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr, + ifm3d::O3R::Ptr cam, ifm3d::PortInfo port_info, std::shared_ptr ifm3d_mutex) + : logger_(logger), node_ptr_(node_ptr), ifm3d_mutex_(ifm3d_mutex), cam_(cam), port_info_(port_info) +{ + RCLCPP_INFO(logger_, "BaseServices constructor called."); + + // + // Set up our service servers + // + this->dump_srv_ = this->node_ptr_->create_service( + "~/Dump", std::bind(&ifm3d_ros2::BaseServices::Dump, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + + this->config_srv_ = this->node_ptr_->create_service( + "~/Config", std::bind(&ifm3d_ros2::BaseServices::Config, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + + this->soft_off_srv_ = this->node_ptr_->create_service( + "~/Softoff", std::bind(&ifm3d_ros2::BaseServices::Softoff, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + + this->soft_on_srv_ = this->node_ptr_->create_service( + "~/Softon", std::bind(&ifm3d_ros2::BaseServices::Softon, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + + this->get_diag_srv_ = this->node_ptr_->create_service( + "~/GetDiag", std::bind(&ifm3d_ros2::BaseServices::GetDiag, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + + RCLCPP_INFO(logger_, "Services created;"); +} + +void BaseServices::Dump(std::shared_ptr request_header, DumpRequest req, DumpResponse resp) +{ + (void)request_header; + (void)req; + RCLCPP_INFO(this->logger_, "Handling dump request..."); + + if (this->node_ptr_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + resp->status = -1; + // XXX: may want to change this logic. For now, I do it so I know + // the ifm3d data structures are not null pointers + RCLCPP_WARN(this->logger_, "Can only make a service request when node is ACTIVE"); + return; + } + + { + std::lock_guard lock(*this->ifm3d_mutex_); + resp->status = 0; + + try + { + json j = this->cam_->ToJSON(); + resp->config = j.dump(); + } + catch (const ifm3d::Error& ex) + { + resp->status = ex.code(); + RCLCPP_WARN(this->logger_, "%s", ex.what()); + } + catch (const std::exception& std_ex) + { + resp->status = -1; + RCLCPP_WARN(this->logger_, "%s", std_ex.what()); + } + catch (...) + { + resp->status = -2; + } + + if (resp->status != 0) + { + RCLCPP_WARN(this->logger_, "Dump: %d", resp->status); + } + } + + RCLCPP_INFO(this->logger_, "Dump request done."); +} + +void BaseServices::Config(const std::shared_ptr /*unused*/, ConfigRequest req, ConfigResponse resp) +{ + RCLCPP_INFO(this->logger_, "Handling config request..."); + + if (this->node_ptr_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + resp->status = -1; + // XXX: may want to change this logic. For now, I do it so I know + // the ifm3d data structures are not null pointers + RCLCPP_WARN(this->logger_, "Can only make a service request when node is ACTIVE"); + return; + } + + { + std::lock_guard lock(*this->ifm3d_mutex_); + resp->status = 0; + resp->msg = "OK"; + + try + { + this->cam_->FromJSON(json::parse(req->json)); // HERE + } + catch (const ifm3d::Error& ex) + { + resp->status = ex.code(); + resp->msg = ex.what(); + } + catch (const std::exception& std_ex) + { + resp->status = -1; + resp->msg = std_ex.what(); + } + catch (...) + { + resp->status = -2; + resp->msg = "Unknown error in `Config'"; + } + + if (resp->status != 0) + { + RCLCPP_WARN(this->logger_, "Config: %d - %s", resp->status, resp->msg.c_str()); + } + } + + RCLCPP_INFO(this->logger_, "Config request done."); +} + +void BaseServices::Softoff(const std::shared_ptr, SoftoffRequest, SoftoffResponse resp) +{ + RCLCPP_INFO(this->logger_, "Handling SoftOff request..."); + + if (this->node_ptr_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + resp->status = -1; + RCLCPP_WARN(this->logger_, "Can only make a service request when node is ACTIVE"); + return; + } + + { + std::lock_guard lock(*this->ifm3d_mutex_); + resp->status = 0; + auto port_type = this->port_info_.type; + try + { + if ((port_type == "2D") | (port_type == "3D")) + { + this->cam_->FromJSONStr(R"({"ports":{")" + this->port_info_.port + R"(":{"state":"CONF"}}})"); + RCLCPP_INFO(this->logger_, "SoftOff request successful."); + } + else if (port_type == "app") + { + this->cam_->FromJSONStr(R"({"applications":{"instances":{")" + this->port_info_.port + + R"(":{"state":"CONF"}}}})"); + RCLCPP_INFO(this->logger_, "SoftOff request successful."); + } + else + { + RCLCPP_WARN(this->logger_, "Unknown port type: %s", port_type.c_str()); + } + } + catch (const ifm3d::Error& ex) + { + resp->status = ex.code(); + RCLCPP_WARN(this->logger_, "Caught ifm3d exception: %s", ex.what()); + } + catch (const std::exception& std_ex) + { + resp->status = -1; + RCLCPP_WARN(this->logger_, "Caught standard exception: %s", std_ex.what()); + } + catch (...) + { + resp->status = -2; + RCLCPP_WARN(this->logger_, "Caught unknown exception"); + } + + if (resp->status != 0) + { + RCLCPP_WARN(this->logger_, "SoftOff: %d", resp->status); + } + } + + RCLCPP_INFO(this->logger_, "SoftOff request done."); +} + +void BaseServices::Softon(const std::shared_ptr /*unused*/, SoftonRequest /*unused*/, + SoftonResponse resp) +{ + RCLCPP_INFO(this->logger_, "Handling SoftOn request..."); + + if (this->node_ptr_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + resp->status = -1; + RCLCPP_WARN(this->logger_, "Can only make a service request when node is ACTIVE"); + return; + } + + { + std::lock_guard lock(*this->ifm3d_mutex_); + resp->status = 0; + auto port_type = this->port_info_.type; + try + { + if ((port_type == "2D") | (port_type == "3D")) + { + this->cam_->FromJSONStr(R"({"ports":{")" + this->port_info_.port + R"(":{"state":"RUN"}}})"); + RCLCPP_INFO(this->logger_, "SoftOff request successful."); + } + else if (port_type == "app") + { + this->cam_->FromJSONStr(R"({"applications":{"instances":{")" + this->port_info_.port + + R"(":{"state":"RUN"}}}})"); + RCLCPP_INFO(this->logger_, "SoftOff request successful."); + } + else + { + RCLCPP_WARN(this->logger_, "Unknown port type: %s", port_type.c_str()); + } + } + catch (const ifm3d::Error& ex) + { + resp->status = ex.code(); + RCLCPP_WARN(this->logger_, "Caught ifm3d exception: %s", ex.what()); + } + catch (const std::exception& std_ex) + { + resp->status = -1; + RCLCPP_WARN(this->logger_, "Caught standard exception: %s", std_ex.what()); + } + catch (...) + { + resp->status = -2; + RCLCPP_WARN(this->logger_, "Caught unknown exception"); + } + + if (resp->status != 0) + { + RCLCPP_WARN(this->logger_, "SoftOff: %d", resp->status); + } + } + + RCLCPP_INFO(this->logger_, "SoftOff request done."); +} + +void BaseServices::GetDiag(std::shared_ptr request_header, GetDiagRequest req, GetDiagResponse resp) +{ + (void)request_header; + RCLCPP_INFO(this->logger_, "Handling GetDiag request..."); + // TODO: shouldn't this be handled by ROS natively? + if (this->node_ptr_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + resp->status = -1; + RCLCPP_WARN(this->logger_, "Can only make a service request when node is ACTIVE"); + return; + } + try + { + json filter = json::parse(req->filter); + RCLCPP_INFO(this->logger_, "Filter: %s", filter.dump().c_str()); + json diagnostics = this->cam_->GetDiagnosticFiltered(filter); + RCLCPP_INFO(this->logger_, "Filtered diagnostics: %s", diagnostics.dump().c_str()); + resp->msg = diagnostics.dump(); + resp->status = 0; + } + catch (const ifm3d::Error& ex) + { + resp->status = ex.code(); + RCLCPP_INFO(this->logger_, "ifm3d error while trying to get the diagnostic: %s", ex.what()); + } + catch (...) + { + resp->status = -2; + RCLCPP_INFO(this->logger_, "Unknown error while trying to get the diagnostic"); + } + RCLCPP_INFO(this->logger_, "GetDiagFiltered request done."); +} + +} // namespace ifm3d_ros2 diff --git a/src/lib/tof_module.cpp b/src/lib/tof_module.cpp new file mode 100644 index 0000000..2af21ef --- /dev/null +++ b/src/lib/tof_module.cpp @@ -0,0 +1,448 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2024 ifm electronic, gmbh + */ + +#include +#include +#include +#include + +#include +#include +#include + +namespace ifm3d_ros2 +{ +TofModule::TofModule(rclcpp::Logger logger, rclcpp_lifecycle::LifecycleNode::SharedPtr node_ptr, + ifm3d::O3R::Ptr o3r_ptr, std::string port, uint32_t width, uint32_t height) + : FunctionModule(logger), node_ptr_(node_ptr), tf_publisher_(node_ptr, o3r_ptr, port), width_(width), height_(height) +{ + RCLCPP_INFO(logger_, "TofModule contructor called."); + + RCLCPP_INFO(this->logger_, "Declaring parameters..."); + const std::string node_name(this->node_ptr_->get_name()); + const std::vector default_buffer_id_list{ + "CONFIDENCE_IMAGE", // + "EXTRINSIC_CALIB", // + "NORM_AMPLITUDE_IMAGE", // + "RADIAL_DISTANCE_IMAGE", // + "TOF_INFO", + "XYZ", // + }; + + buffer_id_list_descriptor_.name = "buffer_id_list"; + buffer_id_list_descriptor_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; + buffer_id_list_descriptor_.description = "List of buffer_id strings denoting the wanted buffers."; + this->node_ptr_->declare_parameter(buffer_id_list_descriptor_.name, default_buffer_id_list, + buffer_id_list_descriptor_); + + tf_base_frame_name_descriptor_.name = "tf.base_frame_name"; + tf_base_frame_name_descriptor_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + tf_base_frame_name_descriptor_.description = "Name for the ifm base frame, defaults to ifm_base_link."; + this->node_ptr_->declare_parameter(tf_base_frame_name_descriptor_.name, "ifm_base_link", + tf_base_frame_name_descriptor_); + + tf_mounting_frame_name_descriptor_.name = "tf.mounting_frame_name"; + tf_mounting_frame_name_descriptor_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + tf_mounting_frame_name_descriptor_.description = + "Name for the mounting point frame, defaults to _mounting_link."; + this->node_ptr_->declare_parameter(tf_mounting_frame_name_descriptor_.name, node_name + "_mounting_link", + tf_mounting_frame_name_descriptor_); + + tf_optical_frame_name_descriptor_.name = "tf.optical_frame_name"; + tf_optical_frame_name_descriptor_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + tf_optical_frame_name_descriptor_.description = + "Name for the point optical frame, defaults to _optical_link."; + this->node_ptr_->declare_parameter(tf_optical_frame_name_descriptor_.name, node_name + "_optical_link", + tf_optical_frame_name_descriptor_); + + tf_publish_base_to_mounting_descriptor_.name = "tf.publish_base_to_mounting"; + tf_publish_base_to_mounting_descriptor_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + tf_publish_base_to_mounting_descriptor_.description = + "Whether the transform from the ifm base link to the mounting point should be published."; + this->node_ptr_->declare_parameter(tf_publish_base_to_mounting_descriptor_.name, true, + tf_publish_base_to_mounting_descriptor_); + + tf_publish_mounting_to_optical_descriptor_.name = "tf.publish_mounting_to_optical"; + tf_publish_mounting_to_optical_descriptor_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + tf_publish_mounting_to_optical_descriptor_.description = + "Whether the transform from the mounting point to the optical frame should be published."; + this->node_ptr_->declare_parameter(tf_publish_mounting_to_optical_descriptor_.name, true, + tf_publish_mounting_to_optical_descriptor_); + RCLCPP_INFO(this->logger_, "After the parameters declaration"); + + this->first_ = true; +} + +void TofModule::handle_frame(ifm3d::Frame::Ptr frame) +{ + RCLCPP_DEBUG(logger_, "Handle TOF frame"); + + using namespace buffer_id_utils; + + RCLCPP_INFO_ONCE(logger_, "Receiving Frames. Processing buffer for [%s]...", + buffer_id_utils::vector_to_string(this->buffer_id_list_).c_str()); + RCLCPP_DEBUG(logger_, "Received new Frame."); + + rclcpp::Time frame_ts = ifm3d_ros2::ifm3d_to_ros_time(frame->TimeStamps()[0]); + RCLCPP_DEBUG(logger_, "Frame timestamp: %f", frame_ts.seconds()); + + auto cloud_header = std_msgs::msg::Header(); + cloud_header.frame_id = tf_publisher_.tf_base_link_frame_name_; + cloud_header.stamp = frame_ts; + + auto optical_header = std_msgs::msg::Header(); + optical_header.frame_id = tf_publisher_.tf_optical_link_frame_name_; + optical_header.stamp = frame_ts; + + for (const ifm3d::buffer_id& id : this->buffer_id_list_) + { + // Helper for logging + auto& clk = *this->node_ptr_->get_clock(); + std::string id_string; + if (!buffer_id_utils::convert(id, id_string)){ + RCLCPP_ERROR(logger_, "Cannot convert the buffer id to a string"); + } + + const buffer_id_utils::message_type message_type = buffer_id_utils::message_type_map[id]; + + if (!frame->HasBuffer(id)) + { + RCLCPP_WARN_THROTTLE(logger_, clk, 5000, + "Frame does not contain buffer %s. Is the correct camera head connected?", + id_string.c_str()); + } + + switch (message_type) + { + case buffer_id_utils::message_type::raw_image: { + auto buffer = frame->GetBuffer(id); + ImageMsg raw_image_msg = ifm3d_ros2::ifm3d_to_ros_image(frame->GetBuffer(id), optical_header, logger_); + image_publishers_[id]->publish(raw_image_msg); + } + break; + + case buffer_id_utils::message_type::pointcloud: { + auto buffer = frame->GetBuffer(id); + PCLMsg pointcloud_msg = ifm3d_ros2::ifm3d_to_ros_cloud(frame->GetBuffer(id), cloud_header, logger_); + pcl_publishers_[id]->publish(pointcloud_msg); + } + break; + case buffer_id_utils::message_type::extrinsics: { + auto buffer = frame->GetBuffer(id); + ExtrinsicsMsg extrinsics_msg = ifm3d_ros2::ifm3d_to_extrinsics(buffer, optical_header, logger_); + extrinsics_publishers_[id]->publish(extrinsics_msg); + } + break; + case buffer_id_utils::intrinsics: { + auto buffer = frame->GetBuffer(id); + IntrinsicsMsg intrinsics_msg = ifm3d_ros2::ifm3d_to_intrinsics(buffer, optical_header, logger_); + intrinsics_publishers_[id]->publish(intrinsics_msg); + } + break; + case buffer_id_utils::message_type::inverse_intrinsics: { + auto buffer = frame->GetBuffer(id); + InverseIntrinsicsMsg inverse_intrinsics_msg = + ifm3d_ros2::ifm3d_to_inverse_intrinsics(buffer, optical_header, logger_); + inverse_intrinsics_publishers_[id]->publish(inverse_intrinsics_msg); + } + break; + case buffer_id_utils::message_type::tof_info: { + auto buffer = frame->GetBuffer(id); + TOFInfoMsg tof_info_msg = ifm3d_ros2::ifm3d_to_tof_info(buffer, optical_header, logger_); + tof_info_publishers_[id]->publish(tof_info_msg); + + if (tf_publisher_.tf_publish_mounting_to_optical_ || tf_publisher_.tf_publish_base_to_mounting_) + { + geometry_msgs::msg::TransformStamped tf_base_to_optical; + if (ifm3d_ros2::ifm3d_tof_info_to_optical_mount_link(buffer, tf_publisher_.tf_base_link_frame_name_, + tf_publisher_.tf_optical_link_frame_name_, logger_, + tf_base_to_optical)) + { + tf_publisher_.update_and_publish_tf_if_changed(tf_base_to_optical); + } + else + { + RCLCPP_ERROR(logger_, "Failed to derive transform from tof_info, retrying..."); + } + } + + CameraInfoMsg camera_info_msg; + if (ifm3d_ros2::ifm3d_tof_info_to_camera_info(buffer, optical_header, this->height_, this->width_, logger_, + camera_info_msg)) + { + RCLCPP_INFO_ONCE(logger_, "Parsing CameraInfo successfull."); + camera_info_publishers_[id]->publish(camera_info_msg); + } + else + { + RCLCPP_ERROR(logger_, "Failed to retrieve camera_info from rgb_info buffer."); + }; + } + break; + default: + RCLCPP_ERROR_THROTTLE(logger_, clk, 5000, "Unknown message type for buffer_id %s. Can not publish.", + id_string.c_str()); + break; + } + } + RCLCPP_DEBUG(logger_, "Finished publishing tof messages."); +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn TofModule::on_configure(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "TofModule: on_configure called"); + (void)previous_state; + + // + // parse params and initialize instance vars + // + RCLCPP_INFO(this->logger_, "Parsing parameters..."); + parse_params(); + RCLCPP_INFO(this->logger_, "Parameters parsed."); + + // Add parameter subscriber, if none is active + if (!param_subscriber_) + { + RCLCPP_INFO(logger_, "Adding callbacks to handle parameter changes at runtime..."); + set_parameter_event_callbacks(); + RCLCPP_INFO(this->logger_, "Callbacks set."); + } + + using namespace buffer_id_utils; + + this->image_publishers_.clear(); + this->pcl_publishers_.clear(); + this->extrinsics_publishers_.clear(); + this->camera_info_publishers_.clear(); + this->tof_info_publishers_.clear(); + this->intrinsics_publishers_.clear(); + this->inverse_intrinsics_publishers_.clear(); + + // Remove buffer_ids unfit for the given data type + this->buffer_id_list_ = + buffer_id_utils::buffer_ids_for_data_stream_type(this->buffer_id_list_, ifm3d_ros2::buffer_id_utils::data_stream_type::tof_3d); + RCLCPP_INFO(logger_, "After removing buffer_ids unfit for the given data stream type, the final list is: [%s].", + buffer_id_utils::vector_to_string(this->buffer_id_list_).c_str()); + + std::vector ids_to_remove{}; + // Create correctly typed publishers for all given buffer_ids + for (const ifm3d::buffer_id& id : this->buffer_id_list_) + { + // Create Publishers in node namespace to make multi-camera setups easier + const std::string topic_name = "~/" + buffer_id_utils::topic_name_map[id]; + const auto qos = ifm3d_ros2::LowLatencyQoS(); + const buffer_id_utils::message_type message_type = buffer_id_utils::message_type_map[id]; + + switch (message_type) + { + case buffer_id_utils::message_type::raw_image: + image_publishers_[id] = node_ptr_->create_publisher(topic_name, qos); + break; + case buffer_id_utils::message_type::pointcloud: + pcl_publishers_[id] = node_ptr_->create_publisher(topic_name, qos); + break; + case buffer_id_utils::message_type::extrinsics: + extrinsics_publishers_[id] = node_ptr_->create_publisher(topic_name, qos); + break; + case buffer_id_utils::message_type::intrinsics: + intrinsics_publishers_[id] = node_ptr_->create_publisher(topic_name, qos); + break; + case buffer_id_utils::message_type::tof_info: + tof_info_publishers_[id] = node_ptr_->create_publisher(topic_name, qos); + camera_info_publishers_[id] = node_ptr_->create_publisher("~/camera_info", qos); + break; + case buffer_id_utils::message_type::inverse_intrinsics: + inverse_intrinsics_publishers_[id] = node_ptr_->create_publisher(topic_name, qos); + break; + default: + std::string id_string; + buffer_id_utils::convert(id, id_string); + RCLCPP_ERROR(logger_, "Unknown message type for buffer_id %s. Will be removed from list...", id_string.c_str()); + ids_to_remove.push_back(id); + break; + } + } + RCLCPP_INFO(this->logger_, "Created publishers for all buffer_ids."); + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn TofModule::on_cleanup(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "TofModule: on_cleanup called"); + (void)previous_state; + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn TofModule::on_shutdown(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "TofModule: on_shutdown called"); + (void)previous_state; + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn TofModule::on_activate(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "TofModule: on_activate called"); + (void)previous_state; + + for (auto& [id, publisher] : this->image_publishers_) + { + publisher->on_activate(); + } + for (auto& [id, publisher] : this->pcl_publishers_) + { + publisher->on_activate(); + } + for (auto& [id, publisher] : this->extrinsics_publishers_) + { + publisher->on_activate(); + } + for (auto& [id, publisher] : this->camera_info_publishers_) + { + publisher->on_activate(); + } + for (auto& [id, publisher] : this->tof_info_publishers_) + { + publisher->on_activate(); + } + for (auto& [id, publisher] : this->intrinsics_publishers_) + { + publisher->on_activate(); + } + for (auto& [id, publisher] : this->inverse_intrinsics_publishers_) + { + publisher->on_activate(); + } + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn TofModule::on_deactivate(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "TofModule: on_deactivate called"); + (void)previous_state; + + for (auto& [id, publisher] : this->image_publishers_) + { + publisher->on_deactivate(); + } + for (auto& [id, publisher] : this->pcl_publishers_) + { + publisher->on_deactivate(); + } + for (auto& [id, publisher] : this->extrinsics_publishers_) + { + publisher->on_deactivate(); + } + for (auto& [id, publisher] : this->camera_info_publishers_) + { + publisher->on_activate(); + } + for (auto& [id, publisher] : this->tof_info_publishers_) + { + publisher->on_deactivate(); + } + for (auto& [id, publisher] : this->intrinsics_publishers_) + { + publisher->on_deactivate(); + } + for (auto& [id, publisher] : this->inverse_intrinsics_publishers_) + { + publisher->on_deactivate(); + } + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::LifecycleNode::CallbackReturn TofModule::on_error(const rclcpp_lifecycle::State& previous_state) +{ + RCLCPP_INFO(logger_, "TofModule: on_error called"); + (void)previous_state; + + return rclcpp_lifecycle::LifecycleNode::CallbackReturn::SUCCESS; +} + +void TofModule::parse_params() +{ + std::vector buffer_id_strings; + this->node_ptr_->get_parameter(buffer_id_list_descriptor_.name, buffer_id_strings); + RCLCPP_INFO(this->logger_, "Reading %ld buffer_ids: [%s]", buffer_id_strings.size(), + buffer_id_utils::vector_to_string(buffer_id_strings).c_str()); + // Populate buffer_id_list_ from read strings + this->buffer_id_list_.clear(); + for (const std::string& string : buffer_id_strings) + { + ifm3d::buffer_id found_id; + if (buffer_id_utils::convert(string, found_id)) + { + this->buffer_id_list_.push_back(found_id); + } + else + { + RCLCPP_WARN(this->logger_, "Ignoring unknown buffer_id %s", string.c_str()); + } + } + RCLCPP_INFO(this->logger_, "Parsed %ld buffer_ids: %s", this->buffer_id_list_.size(), + buffer_id_utils::vector_to_string(this->buffer_id_list_).c_str()); + + this->node_ptr_->get_parameter(tf_base_frame_name_descriptor_.name, tf_publisher_.tf_base_link_frame_name_); + RCLCPP_INFO(this->logger_, "tf.base_link.frame_name: %s", tf_publisher_.tf_base_link_frame_name_.c_str()); + + this->node_ptr_->get_parameter(tf_mounting_frame_name_descriptor_.name, tf_publisher_.tf_mounting_link_frame_name_); + RCLCPP_INFO(this->logger_, "tf.mounting_link.frame_name: %s", tf_publisher_.tf_mounting_link_frame_name_.c_str()); + + this->node_ptr_->get_parameter(tf_optical_frame_name_descriptor_.name, tf_publisher_.tf_optical_link_frame_name_); + RCLCPP_INFO(this->logger_, "tf.optical_link.frame_name: %s", tf_publisher_.tf_optical_link_frame_name_.c_str()); + + this->node_ptr_->get_parameter(tf_publish_base_to_mounting_descriptor_.name, + tf_publisher_.tf_publish_base_to_mounting_); + + this->node_ptr_->get_parameter(tf_publish_mounting_to_optical_descriptor_.name, + tf_publisher_.tf_publish_mounting_to_optical_); + + RCLCPP_INFO(this->logger_, "tf.optical_link.publish_transform: %s", + tf_publisher_.tf_publish_mounting_to_optical_ ? "true" : "false"); +} + +void TofModule::set_parameter_event_callbacks() +{ + // Create a parameter subscriber that can be used to monitor parameter changes + param_subscriber_ = std::make_shared(this->node_ptr_); + + auto buffer_id_list_cb = [this](const rclcpp::Parameter& p) { + RCLCPP_WARN(logger_, "This new buffer_id_list will be used after CONFIGURE transition was called: %s", + buffer_id_utils::vector_to_string(p.as_string_array()).c_str()); + }; + registered_param_callbacks_[buffer_id_list_descriptor_.name] = + param_subscriber_->add_parameter_callback(buffer_id_list_descriptor_.name, buffer_id_list_cb); + + auto tf_mounting_link_frame_name_cb = [this](const rclcpp::Parameter& p) { + tf_publisher_.tf_mounting_link_frame_name_ = p.as_string(); + RCLCPP_INFO(logger_, "New tf.mounting_link.frame_name: '%s'", tf_publisher_.tf_mounting_link_frame_name_.c_str()); + }; + registered_param_callbacks_[tf_mounting_frame_name_descriptor_.name] = param_subscriber_->add_parameter_callback( + tf_mounting_frame_name_descriptor_.name, tf_mounting_link_frame_name_cb); + + auto tf_optical_link_frame_name_cb = [this](const rclcpp::Parameter& p) { + tf_publisher_.tf_optical_link_frame_name_ = p.as_string(); + RCLCPP_INFO(logger_, "New tf.optical_link.frame_name: '%s'", tf_publisher_.tf_optical_link_frame_name_.c_str()); + }; + registered_param_callbacks_[tf_optical_frame_name_descriptor_.name] = + param_subscriber_->add_parameter_callback(tf_optical_frame_name_descriptor_.name, tf_optical_link_frame_name_cb); + + auto tf_optical_link_publish_transform_cb = [this](const rclcpp::Parameter& p) { + tf_publisher_.tf_publish_mounting_to_optical_ = p.as_bool(); + RCLCPP_INFO(logger_, "New tf.optical_link.publish_transform: %s", + tf_publisher_.tf_publish_mounting_to_optical_ ? "true" : "false"); + }; + registered_param_callbacks_[tf_publish_mounting_to_optical_descriptor_.name] = + param_subscriber_->add_parameter_callback(tf_publish_mounting_to_optical_descriptor_.name, + tf_optical_link_publish_transform_cb); +} + +} // namespace ifm3d_ros2 \ No newline at end of file diff --git a/srv/GetDiag.srv b/srv/GetDiag.srv new file mode 100644 index 0000000..63c74cf --- /dev/null +++ b/srv/GetDiag.srv @@ -0,0 +1,4 @@ +string filter +--- +int32 status +string msg