diff --git a/.gitignore b/.gitignore index e476bd8..573510e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,53 @@ #editor temp/backup files *~ *.swp + +## VSCode and Python +.DS_Store +.huskyrc.json +out +log.log +**/node_modules +*.pyc +*.vsix +**/.vscode/.ropeproject/** +**/testFiles/**/.cache/** +*.noseids +.nyc_output +.vscode-test +__pycache__ +npm-debug.log +**/.mypy_cache/** +!yarn.lock +coverage/ +cucumber-report.json +**/.vscode-test/** +**/.vscode test/** +**/.vscode-smoke/** +**/.venv*/ +port.txt +precommit.hook +pythonFiles/lib/** +debug_coverage*/** +languageServer/** +languageServer.*/** +bin/** +obj/** +.pytest_cache +tmp/** +.python-version +.vs/ +test-results*.xml +xunit-test-results.xml +build/ci/performance/performance-results.json +!build/ +debug*.log +debugpy*.log +pydevd*.log +nodeLanguageServer/** +nodeLanguageServer.*/** +dist/** +# translation files +*.xlf +package.nls.*.json +l10n/ \ No newline at end of file diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 85d3889..ce8284a 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,13 +2,46 @@ Changelog for package ifm3d-ros ^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1 (unreleased) +=============== + +1.1.0 +----- +* Update camera nodelet for ifm3d >= v1.2.3 + * Switch from looped image retrieval to callback based image retrieval + * Updated schema mask handling: uint based schema mask replaced with default 2D and 3D bufferID based lists + Between ifm3d API version 0.93 and 1.1 the internal handling of the amplitude and distance image and the point cloud has changed: The previously automatically applied masking based on the confidence image is no longer applied. As a result, more image pixels are displayed as valid pixels. + If you want to apply a binary mapping to these images, use the confidence image as provided in the ROS node and apply the mask `confidence&1` for backward compatibility. + +* Nodelet interface changes: + * additional parameters: + * xyz_image_stream: boolean value - Enable / disable publishing and streaming of point cloud topic / messages. + * confidence_image_stream: boolean value - Enable / disable publishing and streaming of confidence image topic / messages + * radial_distance_image_stream: boolean value - Enable / disable publishing and streaming of radial distance image topic / messages + * radial_distance_noise_stream: boolean value - Enable / disable publishing and streaming of radial distance noise image topic / messages + * amplitude_image_stream: boolean value - Enable / disable publishing and streaming of amplitude image topic / messages + * extrinsic_image_stream: boolean value - Enable / disable publishing and streaming of extrinsics topic / messages + * intrinsic_image_stream: boolean value - Enable / disable publishing and streaming of intrinsics topic / messages + * rgb_image_stream: boolean value - Enable / disable publishing and streaming of 2D RGB image topic / messages + * Updated the default launch files for 2D RGB / 3D TOF cameras accordingly + * Added support for JSON schema dump service + * ifm3d-ros logging: + * Reduced level of logging messages + * Changed default behavior of info log messages: NODELET_INFO_STREAM replaced by NODELET_INFO_ONCE + * Removed: + * Compressed amplitude stream: only amplitude publisher is used + * Unit vector publisher +* Robustness improvements: + * Only publish data to image streams if API frames include the corresponding image data + 1.0 === + 1.0.2 ----- * Fix the IP parameter that was not being used -1.0.1 +1.0.1 ----- * Removed dependency to OpenCV and PCL. diff --git a/README.md b/README.md index 2d54fbf..4d3d2c1 100644 --- a/README.md +++ b/README.md @@ -1,34 +1,48 @@ # ifm3d-ros -## Release versions +*This documentation is formatted to be read on www.ifm3d.com.* -warning:: Note that the `master` branch is generally in a work in progress, and you probably want to use a tagged [release version](https://github.com/ifm/ifm3d-ros/releases) for production. +:::{note} The ifm3d-ros 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. +::: -## ifm3d-ros for the O3R +## Compatibility Matrix +:::{warning} Note that the `master` branch is generally in a work in progress, and you probably want to use a tagged [release version](https://github.com/ifm/ifm3d-ros/releases) for production. +::: + +| **ifm3d-ros version** | **ifm3d version** | **embedded O3R FW version** | **ROS distribution(s)** | +| --------------------- | ----------------- | --------------------------- | ----------------------- | +| 1.1.1 | 1.2.6 | 1.0.14 | ROS Noetic | +| 1.1.0 (unreleased) | 1.2.3 | 1.0.x | ROS Noetic | +| 1.0.1 | 0.93.0 | 0.14.23 | ROS Noetic | +| 1.0.0 | 0.91.0 | 0.14.23 | ROS Noetic | + + +### Internal ifm3d-ros subpackage version structure +Please see the internal subpackage version structure for a known `ifm3d-ros version`. + +| **ifm3d-ros version** | **ifm3d_ros_driver** | **ifm3d_ros_msgs** | **ifm3d_ros_examples** | +| --------------------- | -------------------- | ------------------ | ---------------------- | +| 1.1.1 | 1.1.1 | 0.2.0 | 0.2.0 | +| 1.1.0 (unreleased) | 1.1.0 | 0.2.0 | 0.2.0 | +| 1.0.1 | 1.0.1 | 0.1.0 | 0.1.0 | +| 1.0.0 | 0.7.0 | 0.1.0 | 0.1.0 | -**NOTE: The ifm3d-ros 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.** -We tried to ensure backward compatibility where ever possible. If you find any major breaks, please let us know. +## Changelogs ++ ifm3d-ros: For changes between on the ifm3d-ros node source code, please see the respective information in the [CHANGELOG](CHANGELOG.rst). ++ ifm3d: For changes between on the ifm3d API source code, please see the respective information in the [ifm3d CHANGELOG](https://github.com/ifm/ifm3d/blob/main/ChangeLog.md). ++ O3R firmware: For changes between on the different FW versions, please see the respective information in the [FW release notes](https://ifm3d.com/documentation/Firmware/index.html). + + +## ifm3d-ros for the O3R ifm3d-ros is a wrapper around [ifm3d](ifm3d/doc/sphinx/content/README:ifm3d%20Overview) enabling the usage of the O3R camera platform (ifm ToF cameras) from within [ROS](http://ros.org) software systems. **Please make sure to use version {{ ifm3d_ros_latest_version }} or above for O3R compatibility.** ![rviz1](ifm3d_ros_driver/doc/figures/O3R_merged_point_cloud.png) -## Software Compatibility Matrix - -|**ifm3d-ros version**|**ifm3d version**|**ROS distribution(s)**| -| ------------ | ------------ | ------------ | -| 1.0.0 | 0.91.0 | Noetic | -| 1.0.1 | 0.93.0 | Noetic | +**NOTE** The confidence image handling inside the ifm3d API has changed which results in more valid pixels - for more information see the [Release Notes](Release_notes.md) -### Internal ifm3d-ros subpackage version structure -Please see the internal subpackage version structure for a known `ifm3d-ros version`. - -|**ifm3d-ros version**|**ifm3d_ros_driver**|**ifm3d_ros_msgs**|**ifm3d_ros_examples**| -| ------------ | ------------ | ------------ | ------------ | -| 1.0.0 | 0.7.0 | 0.1.0 | 0.1.0 | -| 1.0.1 | 1.0.1 | 0.1.0 | 0.1.0 | ## Organization of the software @@ -47,5 +61,6 @@ Preparing your system Installing the ifm3d-ros node Build for distributed systems ::: + ## LICENSE Please see the file called [LICENSE](LICENSE). diff --git a/Release_notes.md b/Release_notes.md new file mode 100644 index 0000000..89c8b45 --- /dev/null +++ b/Release_notes.md @@ -0,0 +1,14 @@ +# Release notes ifm3d-ros + +## Dependent software and firmware release notes and changelogs ++ ifm3d-ros: For changes between on the ifm3d-ros node source code, please see the respective information in the [CHANGELOG](CHANGELOG.rst). ++ ifm3d: For changes between on the ifm3d API source code, please see the respective information in the [ifm3d CHANGELOG](https://github.com/ifm/ifm3d/blob/main/ChangeLog.md). ++ O3R firmware: For changes between on the different FW versions, please see the respective information in the [FW release notes](https://ifm3d.com/documentation/Firmware/index.html). + + + +## ifm3d-ros 1.1 +The ifm3d-ros node version 1.1 depends on ifm3d API 1.2.6, as can be seen in the [compatibility matrix](README.md#software-compatibility-matrix). + +Between ifm3d API version 0.93 and 1.1 the internal handling of the amplitude and distance image and the point cloud has changed: The previously automatically applied masking based on the confidence image is no longer applied. As a result, more image pixels are displayed as valid pixels. +If you want to apply a binary mapping to these images, use the confidence image as provided in the ROS node and apply the mask `confidence&1` for backward compatibility. \ No newline at end of file diff --git a/docker/Dockerfile b/docker/Dockerfile new file mode 100644 index 0000000..f5ff299 --- /dev/null +++ b/docker/Dockerfile @@ -0,0 +1,96 @@ +############################################# +# Copyright 2023-present ifm electronic, gmbh +# SPDX-License-Identifier: Apache-2.0 +############################################# +ARG BASE_IMAGE +ARG BUILD_IMAGE_TAG +ARG FINAL_IMAGE_TAG +FROM ${BASE_IMAGE}:${BUILD_IMAGE_TAG} AS build +ARG IFM3D_VERSION +ARG IFM3D_ROS_REPO +ARG IFM3D_ROS_BRANCH +ARG ARCH +ARG UBUNTU_VERSION + +# Create the ifm user +RUN id ifm 2>/dev/null || useradd --uid 30000 --create-home -s /bin/bash -U ifm +WORKDIR /home/ifm + +# Dependencies for both ifm3d and ifm3d-ros2 +ARG DEBIAN_FRONTEND=noninteractive +RUN apt-get update && apt-get install -y \ + git \ + jq \ + libxmlrpc-c++8-dev \ + libproj-dev \ + build-essential \ + coreutils \ + cmake \ + wget \ + libssl-dev \ + libgoogle-glog-dev \ + libgoogle-glog0v5 \ + python3-rosdep + +# 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 +RUN cd /home/ifm/ifm3d &&\ + tar -xf ifm3d-ubuntu-${UBUNTU_VERSION}-${ARCH}-debs_${IFM3D_VERSION}.tar && \ + dpkg -i *.deb + +# Clone and build ifm3d-ros repo +SHELL ["/bin/bash", "-c"] +# RUN mkdir -p /home/ifm/catkin_ws/src && \ +# cd /home/ifm/catkin_ws/src && \ + # git clone ${IFM3D_ROS_REPO} -b ${IFM3D_ROS_BRANCH} --single-branch +ADD . /home/ifm/catkin_ws/src +RUN cd /home/ifm/catkin_ws && \ + rosdep update --rosdistro=${ROS_DISTRO} && \ + rosdep install --from-path src -y --ignore-src + +RUN cd /home/ifm/catkin_ws && \ + source /opt/ros/${ROS_DISTRO}/setup.bash && \ + catkin_make + +# Multistage build to reduce image size +ARG BASE_IMAGE +FROM ${BASE_IMAGE}:${FINAL_IMAGE_TAG} +# Copy files built in previous stage +COPY --from=build /home/ifm/catkin_ws /home/ifm/catkin_ws +COPY --from=build /home/ifm/ifm3d/*.deb /home/ifm/ifm3d/ +WORKDIR /home/ifm + +# Install ifm3d and ifm3d-ros2 runtime dependencies +ARG DEBIAN_FRONTEND=noninteractive +RUN apt-get update \ + && apt-get install -y --no-install-recommends \ + libxmlrpc-c++8v5 \ + locales \ + sudo \ + libssl-dev \ + libgoogle-glog0v5 \ + libboost-all-dev \ + python3-rosdep \ + && rm -rf /var/lib/apt/lists/* + +# Install ifm3d +RUN cd /home/ifm/ifm3d &&\ + dpkg -i *.deb + +RUN cd /home/ifm/catkin_ws && \ + apt-get update && \ + rosdep init && \ + rosdep update --rosdistro=${ROS_DISTRO} && \ + rosdep install --from-path src -y --ignore-src + +# Setup localisation +RUN echo "en_US.UTF-8 UTF-8" >> /etc/locale.gen && \ + locale-gen en_US.UTF-8 && \ + /usr/sbin/update-locale LANG=en_US.UTF-8 + +ENV LANG en_US.UTF-8 +ENV LANGUAGE en_US:en +ENV LC_ALL en_US.UTF-8 + +# NOTE: Make sure to run export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp diff --git a/docker/Dockerfile_buildstage_only b/docker/Dockerfile_buildstage_only new file mode 100644 index 0000000..764582d --- /dev/null +++ b/docker/Dockerfile_buildstage_only @@ -0,0 +1,59 @@ +############################################# +# Copyright 2023-present ifm electronic, gmbh +# SPDX-License-Identifier: Apache-2.0 +############################################# + +ARG BASE_IMAGE +ARG BUILD_IMAGE_TAG +ARG FINAL_IMAGE_TAG +FROM ${BASE_IMAGE}:${BUILD_IMAGE_TAG} AS build +ARG IFM3D_VERSION +ARG IFM3D_ROS_REPO +ARG IFM3D_ROS_BRANCH +ARG ARCH +ARG UBUNTU_VERSION + +# Create the ifm user +RUN id ifm 2>/dev/null || useradd --uid 30000 --create-home -s /bin/bash -U ifm +WORKDIR /home/ifm + +# Dependencies for both ifm3d and ifm3d-ros2 +ARG DEBIAN_FRONTEND=noninteractive +RUN apt-get update && apt-get install -y \ + git \ + jq \ + libxmlrpc-c++8-dev \ + libproj-dev \ + libcurl4 \ + libgoogle-glog-dev \ + libgoogle-glog0v5 \ + build-essential \ + coreutils \ + cmake \ + wget \ + libssl-dev \ + python3-rosdep + +# 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 +RUN cd /home/ifm/ifm3d &&\ + tar -xf ifm3d-ubuntu-${UBUNTU_VERSION}-${ARCH}-debs_${IFM3D_VERSION}.tar && \ + dpkg -i *.deb + +SHELL ["/bin/bash", "-c"] +# Manually install ifm3d-ros additional ROS packages: based on noetic-ros-core base image +RUN apt-get update && apt-get install -y \ + ros-noetic-tf2-ros \ + ros-noetic-tf \ + ros-noetic-image-transport + +ADD . /home/ifm/catkin_ws/src + +RUN cd /home/ifm/catkin_ws && \ + source /opt/ros/${ROS_DISTRO}/setup.bash && \ + catkin_make --only-pkg-with-deps ifm3d_ros_msgs + +RUN cd /home/ifm/catkin_ws && \ + source /opt/ros/${ROS_DISTRO}/setup.bash && \ + catkin_make --only-pkg-with-deps ifm3d-ros \ No newline at end of file diff --git a/docker/README_VPU_docker_compose_setup.md b/docker/README_VPU_docker_compose_setup.md new file mode 100644 index 0000000..93d4ad7 --- /dev/null +++ b/docker/README_VPU_docker_compose_setup.md @@ -0,0 +1,202 @@ + +# ROS network: Docker container + +There are several objectives and limitations to consider when running a ifm3d-ros Docker container on the VPU: + +There are two goals that must be met when running a Docker container directly on the VPU: +1. Use `IFM3D_IP=127.0.0.1` for local communication to avoid network drop propagation of ETH0 into the container. +2. Allow communication between ROS nodes through the localhost / default gateway (0.0.0.0) instead of the Ethernet interface ETH0. + +All Docker processes should run in a single Docker container instance. Only by running all ROS processes in the same Docker context can the communication overhead be limited. +This communication overhead consists of serializing, sending ROS data frames over the network, and de-serializing this data at each ROS receiver. + +The ifm3d-ros nodes are designed in a ROS nodelet architecture: To take advantage of the nodelet performance gains, all dependent ROS nodes should also be implemented as ROS nodelets. +Only then the full advantage of a nodelet architecture can be used: i.e. passing data reference pointers instead of serializing / deserializing the data to be sent in the network. + +Please see the solutions below to setup a network communication to allow running a ifm3d-ros node in a Docker container on the VPU. + +## Solution 1: localhost based communication - network host + +**Limitations of ROS in a Docker container context** ++ ifm3d camera communication: use localhost - 127.0.0.1 to avoid ETH0 network drops resulting in ifm3d objects re-init + + (192.168.0.69 Interface used for ROS communications is not feasible: all network connection drops get propagated into the container / ROS master and node) ++ manual port selection for roslaunch is not possible: + + Use `network host` to share the host network to the Docker container during runtime ++ Since the ROS master and node communication can not be separated for the network interfaces: + + both have to communicate on the same network - default route: 0.0.0.0 interface + +**Alternative:** ++ 2 ROS Masters and a ROS bridge to connect them: This however is not feasible as ROS bridges are not designed to handle large datasets such as point cloud data. + +**Solution** +**Please do not route the ROS communication via the Default gateway - a communication via ETH0 (192.168.0.0 subnet) is highly discouraged as its routing tables will change when connections on ETH0 NIC are established or closed!** + ++ For name resolution manually modify the /etc/hosts + Docker container + ``` + 127.0.0.1 localhost.localdomain localhost + 0.0.0.0 ifm3d_rosmaster + 0.0.0.0 ifm3d_ros + ``` + ++ IPC / Laptop: /etc/hosts + ``` + 192.168.0.69 ifm3d_rosmaster + 192.168.0.69 ifm3d_ros + ``` + +During Runtime these parameters env variables have to be set: ++ Docker Containers: +``` +export IFM3D_IP="127.0.0.1" +export ROS_MASTER_URI=http://ifm3d_rosmaster:11311 +export ROS_HOSTNAME=ifm3d_ros +``` ++ Laptop IPC: +``` +export ROS_MASTER_URI=http://ifm3d_rosmaster:11311 +export ROS_HOSTNAME=ifm3d_ros_listener +``` + +The corresponding DNS `/etc/hosts` - see above + +Dockerfile +```yml +version: "2.3" +services: + ifm3d_ros: + tty: true + image: ifm3d-ros:noetic-arm64_v8 + + restart: unless-stopped + volumes: + - ./launch/four_cameras.launch:/home/ifm/catkin_ws/src/ifm3d_ros_driver/launch/four_cameras.launch + - ./hosts_ros_master:/etc/hosts/ + + logging: + driver: "json-file" + options: + mode: non-blocking + max-buffer-size: "20m" + + command: + - /bin/bash + - -c + - | + set -a + . /opt/ros/noetic/setup.sh; + . /home/ifm/catkin_ws/devel/setup.sh; + export IFM3D_IP="127.0.0.1"; + export ROS_MASTER_URI=http://ifm3d_rosmaster:11311; + export ROS_HOSTNAME=ifm3d_ros; + echo $$ROS_MASTER_URI; + echo $$ROS_IP; + roslaunch ifm3d_ros_driver four_cameras.launch --wait -t 10 + + network_mode: host +``` + +## Solution 2: separate Docker network + +**Motivation** +Separate the two networks: localhost and dedicated Docker network for ROS container communication on the VPU. +This will solve the problem of all ROS communication being shared on the same node. + +**Limitations**. +Please note that the following changes may result in routing tables that are not functional. This may cause the VPU to be inaccessible via its Ethernet interface. Please proceed with caution. + +**When creating/modifying networks on the VPU, please consider your existing network topology and possible conflicts!**. + +**Docker compose including network setup:**. +This includes setting up a dedicated `ros_net` Docker network for shared communication between Docker containers. + +```yml +version: "2.3" +services: + ifm3d_ros: + tty: true + image: ifm3d-ros:noetic-arm64_v8 + + extra_hosts: + - "dockerhost:172.20.0.1" + + networks: + ros_net: + ipv4_address: 172.20.0.2 + + ports: + - 11311:11311 + + + restart: unless-stopped + volumes: + - ./launch/four_cameras.launch:/home/ifm/catkin_ws/src/ifm3d_ros_driver/launch/four_cameras.launch + logging: + driver: "json-file" + options: + mode: non-blocking + max-buffer-size: "20m" + + command: + - /bin/bash + - -c + - | + (. /opt/ros/noetic/setup.sh; + . /home/ifm/catkin_ws/devel/setup.sh; + export ROS_MASTER_URI=http://172.20.0.2:11311; + export ROS_HOSTNAME=172.20.0.2; + echo ROS_MASTER_URI=$$ROS_MASTER_URI; + echo ROS_HOSTNAME=$$ROS_HOSTNAME; + roscore & + roslaunch ifm3d_ros_driver four_cameras.launch --wait -t 10) + +networks: + ros_net: + ipam: + driver: default + config: + - subnet: 172.20.0.0/16 + gateway: 172.20.0.1 + +``` + +Depending on Docker and the docker-compose version a first initial manual setup of such a Docker network might be required: +```shell +docker network create --gateway 172.20.0.1 --subnet 172.20.0.0/24 ros_net +``` + +The resulting routing table +``` +Destination Gateway Genmask Flags Metric Ref Use Iface +0.0.0.0 192.168.0.201 0.0.0.0 UG 0 0 0 eth0 +172.17.0.0 0.0.0.0 255.255.0.0 U 0 0 0 docker0 +172.20.0.0 0.0.0.0 255.255.0.0 U 0 0 0 br-5866d929f186 +192.168.0.0 0.0.0.0 255.255.255.0 U 0 0 0 eth0 +``` + + +To allow communication between this dedicated Docker network one has to attach this virtual interface (the Docker network) to the ETH0 interface: +**THIS IS CURRENTLY ONLY POSSIBLE AS root user and NOT as oem user** +```bash +brctl show +bridge name bridge id STP enabled interfaces +br-5866d929f186 8000.02428756949b no eth0 + vethd1fc3b6 + +``` +Add the virtual interface to ETH0 interface +```bash +brctl addif br-5866d929f186 eth0 +``` + +**IPC / Host machine** +To listen to ifm3d-ros topics and services (additionally to the ROS masters topics) an additional route has to be configured on the host: +```bash +sudo ip route add 172.20.0.1/24 via 192.168.0.69 +``` + +After exporting the correct `ROS_MASTER_URI` and `ROS_HOSTNAME` the communication with the ROS core and ifm3d-ros node should be possible: +```bash +export ROS_MASTER_URI=http://172.20.0.1:11311 +export ROS_HOSTNAME=172.20.0.1 +``` \ No newline at end of file diff --git a/docker/build_container.sh b/docker/build_container.sh new file mode 100755 index 0000000..e696125 --- /dev/null +++ b/docker/build_container.sh @@ -0,0 +1,40 @@ +#!/bin/bash +set -euo pipefail + +############## +# For AMD64: +############## +ARCH="amd64" +BASE_IMAGE="amd64/ros" +TAG=ifm3d-ros_126:noetic-amd64_1.1.1 + +############## +# For ARM64V8: +############## +# ARCH="arm64v8" +# BASE_IMAGE="arm64v8/ros" +# TAG=ifm3d-ros_126:noetic-arm64_v8_1.1.1 + +############## +# Arguments common for both architecture +############## +BUILD_IMAGE_TAG="noetic" +FINAL_IMAGE_TAG="noetic-ros-core" +IFM3D_VERSION="1.2.6" +IFM3D_ROS_REPO="https://github.com/ifm/ifm3d-ros" +IFM3D_ROS_BRANCH="dev1.2" +UBUNTU_VERSION="20.04" + +############## +# Build the Docker container +############## +docker build -t $TAG \ + --build-arg ARCH=${ARCH} \ + --build-arg BASE_IMAGE=${BASE_IMAGE} \ + --build-arg BUILD_IMAGE_TAG=${BUILD_IMAGE_TAG} \ + --build-arg FINAL_IMAGE_TAG=${FINAL_IMAGE_TAG} \ + --build-arg IFM3D_VERSION=${IFM3D_VERSION} \ + --build-arg IFM3D_ROS_REPO=${IFM3D_ROS_REPO} \ + --build-arg IFM3D_ROS_BRANCH=${IFM3D_ROS_BRANCH} \ + --build-arg UBUNTU_VERSION=${UBUNTU_VERSION} \ + -f Dockerfile_buildstage_only . diff --git a/docker/docker-compose_IPC.yml b/docker/docker-compose_IPC.yml new file mode 100644 index 0000000..f5d3ff4 --- /dev/null +++ b/docker/docker-compose_IPC.yml @@ -0,0 +1,30 @@ +version: "2.3" +services: + + ifm3d_ros: + tty: true + image: ifm3d-ros_126:noetic-amd64_1.1.1 + restart: unless-stopped + volumes: + - ./hosts_IPC:/etc/hosts/ + + logging: + driver: "json-file" + options: + mode: non-blocking + max-buffer-size: "20m" + + command: + - /bin/bash + - -c + - | + set -a + . /opt/ros/noetic/setup.sh; + . /home/ifm/catkin_ws/devel/setup.sh; + export ROS_MASTER_URI=http://ifm3d_rosmaster:11311; + export ROS_HOSTNAME=ifm3d_ros; + echo ROS_MASTER_URI=$$ROS_MASTER_URI; + echo ROS_HOSTNAME=$$ROS_HOSTNAME; + roslaunch ifm3d_ros_driver camera_3d.launch pcic_port:=50012 + + network_mode: host diff --git a/docker/docker-compose_VPU.yml b/docker/docker-compose_VPU.yml new file mode 100644 index 0000000..c31465c --- /dev/null +++ b/docker/docker-compose_VPU.yml @@ -0,0 +1,33 @@ +version: "2.3" +services: + ifm3d_ros: + tty: true + image: ifm3d-ros_126:noetic-arm64_v8_1.1.1 + + restart: unless-stopped + volumes: + - ./hosts_ros_master:/etc/hosts/ + + logging: + driver: "json-file" + options: + mode: non-blocking + max-buffer-size: "20m" + + command: + - /bin/bash + - -c + - | + set -a + . /opt/ros/noetic/setup.sh; + . /home/ifm/catkin_ws/devel/setup.sh; + export IFM3D_IP="127.0.0.1"; + export GLOG_logtostderr=1; + export GLOG_minloglevel=3; + export ROS_MASTER_URI=http://ifm3d_rosmaster:11311; + export ROS_HOSTNAME=ifm3d_ros; + echo ROOS_MASTER_URI=$$ROS_MASTER_URI; + echo ROS_HOSTNAME=$$ROS_HOSTNAME; + roslaunch ifm3d_ros_driver camera_3d.launch pcic_port:=50012 + + network_mode: host diff --git a/docker/docker-compose_docker_network.yml b/docker/docker-compose_docker_network.yml new file mode 100644 index 0000000..c1f7b1e --- /dev/null +++ b/docker/docker-compose_docker_network.yml @@ -0,0 +1,44 @@ +version: "2.3" +services: + ifm3d_ros: + tty: true + image: ifm3d-ros_126:noetic-arm64_v8_1.1.1 + + extra_hosts: + - "dockerhost:172.20.0.1" + + networks: + ros_net: + ipv4_address: 172.20.0.2 + + ports: + - 11311:11311 + + + restart: unless-stopped + logging: + driver: "json-file" + options: + mode: non-blocking + max-buffer-size: "20m" + + command: + - /bin/bash + - -c + - | + (. /opt/ros/noetic/setup.sh; + . /home/ifm/catkin_ws/devel/setup.sh; + export ROS_MASTER_URI=http://172.20.0.2:11311; + export ROS_HOSTNAME=172.20.0.2; + echo ROS_MASTER_URI=$$ROS_MASTER_URI; + echo ROS_HOSTNAME=$$ROS_HOSTNAME; + roscore & + roslaunch ifm3d_ros_driver camera_3d.launch pcic_port:=50012) + +networks: + ros_net: + ipam: + driver: default + config: + - subnet: 172.20.0.0/16 + gateway: 172.20.0.1 diff --git a/docker/four_cameras.launch b/docker/four_cameras.launch new file mode 100644 index 0000000..7c324b8 --- /dev/null +++ b/docker/four_cameras.launch @@ -0,0 +1,140 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + " + " + " + " + " + " + " + + + + + + + + + + + + + + + + + + " + " + " + " + " + " + " + + + + + + + + + + + + + + + + + + " + " + " + " + " + " + " + + + + + + + + + + + + + + + + + + " + " + " + " + " + " + " + + + diff --git a/docker/hosts_IPC b/docker/hosts_IPC new file mode 100644 index 0000000..edcf3de --- /dev/null +++ b/docker/hosts_IPC @@ -0,0 +1,3 @@ +127.0.0.1 localhost.localdomain localhost +192.168.0.100 ifm3d_rosmaster +192.168.0.69 ifm3d_ros diff --git a/docker/hosts_ros_master b/docker/hosts_ros_master new file mode 100644 index 0000000..a6b4315 --- /dev/null +++ b/docker/hosts_ros_master @@ -0,0 +1,5 @@ +127.0.0.1 localhost.localdomain localhost +# 0.0.0.0 ifm3d_rosmaster +# 0.0.0.0 ifm3d_ros +192.168.0.69 ifm3d_rosmaster +192.168.0.69 ifm3d_ros diff --git a/docker/ros-entrypoint.sh b/docker/ros-entrypoint.sh new file mode 100755 index 0000000..4d23c61 --- /dev/null +++ b/docker/ros-entrypoint.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +source /opt/ros/noetic/setup.bash +source /home/ifm/catkin_ws/ifm3d-ros/devel/setup.bash +exec "$@" \ No newline at end of file diff --git a/ifm3d-ros/package.xml b/ifm3d-ros/package.xml index 19706b8..f5ecea6 100644 --- a/ifm3d-ros/package.xml +++ b/ifm3d-ros/package.xml @@ -2,12 +2,11 @@ ifm3d-ros - 1.0.1 + 1.1.1 metapackage for the ifm3d-ros package group which interacts with the ifm 3D ToF Camera ROS package - ifm CSR group + ifm robotics group Apache 2 - CSR ifm sytron - + ifm robotics group https://github.com/ifm/ifm3d-ros/ catkin diff --git a/ifm3d_ros_driver/CMakeLists.txt b/ifm3d_ros_driver/CMakeLists.txt index 28c013b..015e878 100644 --- a/ifm3d_ros_driver/CMakeLists.txt +++ b/ifm3d_ros_driver/CMakeLists.txt @@ -1,10 +1,13 @@ cmake_minimum_required(VERSION 3.5) project(ifm3d_ros_driver) -find_package(ifm3d REQUIRED COMPONENTS - camera +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +find_package(ifm3d 1.2.6 REQUIRED COMPONENTS + device framegrabber - stlimage ) find_package(catkin REQUIRED COMPONENTS @@ -35,9 +38,6 @@ catkin_package( ############# ## Build ## ############# -set(CMAKE_CXX_EXTENSIONS OFF) -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED true) include_directories( include @@ -47,11 +47,10 @@ include_directories( add_library(ifm3d_ros src/camera_nodelet.cpp) target_link_libraries(ifm3d_ros ${catkin_LIBRARIES} - ifm3d::camera + ifm3d::device ifm3d::framegrabber - ifm3d::stlimage ) - + ############# ## Install ## ############# diff --git a/ifm3d_ros_driver/include/ifm3d_ros_driver/camera_nodelet.h b/ifm3d_ros_driver/include/ifm3d_ros_driver/camera_nodelet.h index d83a9a4..0a183a1 100644 --- a/ifm3d_ros_driver/include/ifm3d_ros_driver/camera_nodelet.h +++ b/ifm3d_ros_driver/include/ifm3d_ros_driver/camera_nodelet.h @@ -10,16 +10,19 @@ #include #include #include +#include #include #include #include -#include +#include +#include #include -#include +#include #include #include +#include #include #include #include @@ -45,6 +48,7 @@ class CameraNodelet : public nodelet::Nodelet // ROS services // bool Dump(ifm3d_ros_msgs::Dump::Request& req, ifm3d_ros_msgs::Dump::Response& res); + bool DumpJSONSchema(ifm3d_ros_msgs::DumpJSONSchema::Request& req, ifm3d_ros_msgs::DumpJSONSchema::Response& res); bool Config(ifm3d_ros_msgs::Config::Request& req, ifm3d_ros_msgs::Config::Response& res); bool Trigger(ifm3d_ros_msgs::Trigger::Request& req, ifm3d_ros_msgs::Trigger::Response& res); bool SoftOff(ifm3d_ros_msgs::SoftOff::Request& req, ifm3d_ros_msgs::SoftOff::Response& res); @@ -54,8 +58,11 @@ class CameraNodelet : public nodelet::Nodelet // This is our main publishing loop and its helper functions // void Run(); - bool InitStructures(std::uint16_t mask, std::uint16_t pcic_port); - bool AcquireFrame(); + bool InitStructures(std::uint16_t pcic_port); + void Callback2D(ifm3d::Frame::Ptr frame); + void Callback3D(ifm3d::Frame::Ptr frame); + bool StartStream(); + std::string GetCameraType(std::uint16_t); // // state @@ -64,7 +71,23 @@ class CameraNodelet : public nodelet::Nodelet std::uint16_t xmlrpc_port_; std::uint16_t pcic_port_; std::string password_; - std::uint16_t schema_mask_; + std::string imager_type_; + ifm3d::TimePointT last_frame_time_; + ros::Time last_frame_local_time_; + + bool xyz_image_stream_; + bool confidence_image_stream_; + bool radial_distance_image_stream_; + bool radial_distance_noise_stream_; + bool amplitude_image_stream_; + bool extrinsic_image_stream_; + bool intrinsic_image_stream_; + bool rgb_image_stream_; + + std::list buffer_list; + ifm3d::FrameGrabber::BufferList schema_mask_default_3d_; + ifm3d::FrameGrabber::BufferList schema_mask_default_2d_; + int timeout_millis_; double timeout_tolerance_secs_; bool assume_sw_triggered_; @@ -77,10 +100,10 @@ class CameraNodelet : public nodelet::Nodelet std::string frame_id_; std::string optical_frame_id_; - ifm3d::CameraBase::Ptr cam_; + ifm3d::Device::Ptr cam_; ifm3d::FrameGrabber::Ptr fg_; - ifm3d::StlImageBuffer::Ptr im_; std::mutex mutex_; + bool receiving_; ros::NodeHandle np_; std::unique_ptr it_; @@ -91,6 +114,7 @@ class CameraNodelet : public nodelet::Nodelet ros::Publisher cloud_pub_; ros::Publisher uvec_pub_; ros::Publisher extrinsics_pub_; + ros::Publisher intrinsics_pub_; image_transport::Publisher distance_pub_; image_transport::Publisher distance_noise_pub_; image_transport::Publisher amplitude_pub_; @@ -103,6 +127,7 @@ class CameraNodelet : public nodelet::Nodelet // Services we advertise // ros::ServiceServer dump_srv_; + ros::ServiceServer dump_json_schema_srv_; ros::ServiceServer config_srv_; ros::ServiceServer trigger_srv_; ros::ServiceServer soft_off_srv_; @@ -113,6 +138,13 @@ class CameraNodelet : public nodelet::Nodelet // ros::Timer publoop_timer_; + // + // Message header used for publishing + // + std_msgs::Header optical_head; + std_msgs::Header head; + + }; // end: class CameraNodelet } // namespace ifm3d_ros diff --git a/ifm3d_ros_driver/launch/camera_2d.launch b/ifm3d_ros_driver/launch/camera_2d.launch new file mode 100644 index 0000000..f72601a --- /dev/null +++ b/ifm3d_ros_driver/launch/camera_2d.launch @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + " + " + " + " + " + " + " + + + diff --git a/ifm3d_ros_driver/launch/camera.launch b/ifm3d_ros_driver/launch/camera_3d.launch similarity index 56% rename from ifm3d_ros_driver/launch/camera.launch rename to ifm3d_ros_driver/launch/camera_3d.launch index a64cff0..020fbe4 100644 --- a/ifm3d_ros_driver/launch/camera.launch +++ b/ifm3d_ros_driver/launch/camera_3d.launch @@ -7,12 +7,19 @@ - + + + + + + + + @@ -20,12 +27,20 @@ - + + + " + " + " + " + " + " + " diff --git a/ifm3d_ros_driver/launch/nodelet.launch b/ifm3d_ros_driver/launch/nodelet.launch index c48f03f..4929ff6 100644 --- a/ifm3d_ros_driver/launch/nodelet.launch +++ b/ifm3d_ros_driver/launch/nodelet.launch @@ -7,12 +7,20 @@ - + + + + + + + + + @@ -105,6 +154,17 @@ + + + + " + " + " + " + " + " + " + diff --git a/ifm3d_ros_driver/package.xml b/ifm3d_ros_driver/package.xml index 08008e2..86da3a7 100644 --- a/ifm3d_ros_driver/package.xml +++ b/ifm3d_ros_driver/package.xml @@ -2,11 +2,11 @@ ifm3d_ros_driver - 1.0.2 + 1.1.1 ifm 3D ToF Camera ROS main driver package - ifm CSR group + ifm robotics group Apache 2 - CSR ifm sytron + ifm robotics group https://github.com/ifm/ifm3d-ros/ catkin diff --git a/ifm3d_ros_driver/src/camera_nodelet.cpp b/ifm3d_ros_driver/src/camera_nodelet.cpp index 021121a..b05d599 100644 --- a/ifm3d_ros_driver/src/camera_nodelet.cpp +++ b/ifm3d_ros_driver/src/camera_nodelet.cpp @@ -3,6 +3,7 @@ * Copyright (C) 2021 ifm electronic, gmbh */ +#include #include #include @@ -13,6 +14,8 @@ #include #include #include +#include +#include #include #include @@ -30,9 +33,31 @@ #include #include -#include -sensor_msgs::Image ifm3d_to_ros_image(ifm3d::Image& image, // Need non-const image because image.begin(), + +using json = ifm3d::json; +using namespace std::chrono_literals; +// This function formats the timestamps for proper display +// a.k.a converts to local time +std::string formatTimestamp(ifm3d::TimePointT timestamp) +{ + using namespace std::chrono; + std::time_t time = std::chrono::system_clock::to_time_t( + std::chrono::time_point_cast( + timestamp)); + + milliseconds milli = duration_cast( + timestamp.time_since_epoch() - + duration_cast(timestamp.time_since_epoch())); + + std::ostringstream s; + s << std::put_time(std::localtime(&time), "%Y-%m-%d %H:%M:%S") << ":" + << std::setw(3) << std::setfill('0') << milli.count(); + + return s.str(); +} + +sensor_msgs::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::Header& header, const std::string& logger) { @@ -93,12 +118,12 @@ sensor_msgs::Image ifm3d_to_ros_image(ifm3d::Image& image, // Need non-const im return result; } -sensor_msgs::Image ifm3d_to_ros_image(ifm3d::Image&& image, const std_msgs::Header& header, const std::string& logger) +sensor_msgs::Image ifm3d_to_ros_image(ifm3d::Buffer&& image, const std_msgs::Header& header, const std::string& logger) { return ifm3d_to_ros_image(image, header, logger); } -sensor_msgs::CompressedImage ifm3d_to_ros_compressed_image(ifm3d::Image& image, // Need non-const image because +sensor_msgs::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::Header& header, @@ -123,16 +148,26 @@ sensor_msgs::CompressedImage ifm3d_to_ros_compressed_image(ifm3d::Image& image, return result; } -sensor_msgs::CompressedImage ifm3d_to_ros_compressed_image(ifm3d::Image&& image, const std_msgs::Header& header, +sensor_msgs::CompressedImage ifm3d_to_ros_compressed_image(ifm3d::Buffer&& image, const std_msgs::Header& header, const std::string& format, const std::string& logger) { return ifm3d_to_ros_compressed_image(image, header, format, logger); } -sensor_msgs::PointCloud2 ifm3d_to_ros_cloud(ifm3d::Image& image, // Need non-const image because image.begin(), +sensor_msgs::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::Header& header, const std::string& logger) { + // Accessing the header content + uint32_t header_seq = header.seq; + ros::Time header_stamp = header.stamp; + std::string header_frame_id = header.frame_id; + + // Logging header content to DEBUG level using ROS_DEBUG + ROS_DEBUG("Header Sequence: %d", header_seq); + ROS_DEBUG("Header Stamp: %d.%d", header_stamp.sec, header_stamp.nsec); + ROS_DEBUG("Header Frame ID: %s", header_frame_id.c_str()); + sensor_msgs::PointCloud2 result{}; result.header = header; result.height = image.height(); @@ -183,19 +218,24 @@ sensor_msgs::PointCloud2 ifm3d_to_ros_cloud(ifm3d::Image& image, // Need non-co return result; } -sensor_msgs::PointCloud2 ifm3d_to_ros_cloud(ifm3d::Image&& image, const std_msgs::Header& header, +sensor_msgs::PointCloud2 ifm3d_to_ros_cloud(ifm3d::Buffer&& image, const std_msgs::Header& header, const std::string& logger) { return ifm3d_to_ros_cloud(image, header, logger); } -using json = nlohmann::json; +using json = ifm3d::json; namespace enc = sensor_msgs::image_encodings; +inline const char * const BoolToString(bool b) +{ + return b ? "true" : "false"; +} + void ifm3d_ros::CameraNodelet::onInit() { std::string nn = this->getName(); - NODELET_DEBUG_STREAM("onInit(): " << nn); + NODELET_INFO_STREAM("onInit(): " << nn); this->np_ = getMTPrivateNodeHandle(); this->it_.reset(new image_transport::ImageTransport(this->np_)); @@ -203,13 +243,19 @@ void ifm3d_ros::CameraNodelet::onInit() // // parse data out of the parameter server // - // NOTE: AFAIK, there is no way to get an unsigned int type out of the ROS - // parameter server. - // - int schema_mask; + int xmlrpc_port; int pcic_port; std::string frame_id_base; + bool xyz_image_stream; + bool confidence_image_stream; + bool radial_distance_image_stream; + bool radial_distance_noise_stream; + bool norm_amplitude_image_stream; + bool amplitude_image_stream; + bool extrinsic_image_stream; + bool intrinsic_image_stream; + bool rgb_image_stream; if ((nn.size() > 0) && (nn.at(0) == '/')) { @@ -221,14 +267,9 @@ void ifm3d_ros::CameraNodelet::onInit() } this->np_.param("ip", this->camera_ip_, ifm3d::DEFAULT_IP); - NODELET_INFO("IP default: %s, current %s", ifm3d::DEFAULT_IP.c_str(), this->camera_ip_.c_str()); - this->np_.param("xmlrpc_port", xmlrpc_port, (int)ifm3d::DEFAULT_XMLRPC_PORT); this->np_.param("pcic_port", pcic_port, (int)ifm3d::DEFAULT_PCIC_PORT); - NODELET_INFO("pcic port check: current %d, default %d", pcic_port, ifm3d::DEFAULT_PCIC_PORT); - this->np_.param("password", this->password_, ifm3d::DEFAULT_PASSWORD); - this->np_.param("schema_mask", schema_mask, (int)ifm3d::DEFAULT_SCHEMA_MASK); this->np_.param("timeout_millis", this->timeout_millis_, 500); this->np_.param("timeout_tolerance_secs", this->timeout_tolerance_secs_, 5.0); this->np_.param("assume_sw_triggered", this->assume_sw_triggered_, false); @@ -239,38 +280,128 @@ void ifm3d_ros::CameraNodelet::onInit() this->np_.param("frame_latency_thresh", this->frame_latency_thresh_, 60.0f); this->np_.param("frame_id_base", frame_id_base, frame_id_base); + // image stream ros parameter server setting + this->np_.param("xyz_image_stream", xyz_image_stream, true); + this->np_.param("confidence_image_stream", confidence_image_stream, true); + this->np_.param("radial_distance_image_stream", radial_distance_image_stream, true); + this->np_.param("radial_distance_noise_stream", radial_distance_noise_stream, true); + this->np_.param("amplitude_image_stream", amplitude_image_stream, true); + this->np_.param("extrinsic_image_stream", extrinsic_image_stream, true); + this->np_.param("intrinsic_image_stream", intrinsic_image_stream, true); + this->np_.param("rgb_image_stream", rgb_image_stream, true); + + // default schema masks + std::list buffer_list; + const ifm3d::FrameGrabber::BufferList DEFAULT_SCHEMA_MASK_3D = { + ifm3d::buffer_id::XYZ, + ifm3d::buffer_id::CONFIDENCE_IMAGE, + ifm3d::buffer_id::RADIAL_DISTANCE_IMAGE, + ifm3d::buffer_id::RADIAL_DISTANCE_NOISE, + ifm3d::buffer_id::NORM_AMPLITUDE_IMAGE, + ifm3d::buffer_id::EXTRINSIC_CALIB, + ifm3d::buffer_id::INTRINSIC_CALIB, + }; + + const ifm3d::FrameGrabber::BufferList DEFAULT_SCHEMA_MASK_2D = { + ifm3d::buffer_id::JPEG_IMAGE, + ifm3d::buffer_id::EXTRINSIC_CALIB, + ifm3d::buffer_id::INTRINSIC_CALIB, + ifm3d::buffer_id::RGB_INFO, + }; + + this->xyz_image_stream_ = static_cast(xyz_image_stream); + this->confidence_image_stream_ = static_cast(confidence_image_stream); + this->radial_distance_image_stream_ = static_cast(radial_distance_image_stream); + this->radial_distance_noise_stream_ = static_cast(radial_distance_noise_stream); + this->amplitude_image_stream_ = static_cast(amplitude_image_stream); + this->rgb_image_stream_ = static_cast(rgb_image_stream); + this->extrinsic_image_stream_ = static_cast(extrinsic_image_stream); + this->intrinsic_image_stream_ = static_cast(intrinsic_image_stream); + this->xmlrpc_port_ = static_cast(xmlrpc_port); - this->schema_mask_ = static_cast(schema_mask); this->pcic_port_ = static_cast(pcic_port); + NODELET_INFO_ONCE("IP default: %s, current %s", ifm3d::DEFAULT_IP.c_str(), this->camera_ip_.c_str()); + NODELET_INFO_ONCE("PCIC port check: current %d, default %d", this->pcic_port_, ifm3d::DEFAULT_PCIC_PORT); + NODELET_INFO_ONCE("XML-RPC port check: current %d, default %d", this->xmlrpc_port_, ifm3d::DEFAULT_XMLRPC_PORT); + + + this->schema_mask_default_3d_ = DEFAULT_SCHEMA_MASK_3D; // use DEFAULT_SCHEMA_MASK until implemented as yml file: list of strings + this->schema_mask_default_2d_ = DEFAULT_SCHEMA_MASK_2D; // use DEFAULT_SCHEMA_MASK until implemented as yml file: list of strings + + // lastly get the camera type based on PortsInfo + this->np_.param("imager_type", this->imager_type_ , std::string("3D")); + this->imager_type_ = GetCameraType(this->pcic_port_); + + NODELET_INFO_ONCE("Imager type current: %s, default %s", imager_type_.c_str(), "3D"); NODELET_DEBUG_STREAM("setup ros node parameters finished"); + this->frame_id_ = frame_id_base + "_link"; this->optical_frame_id_ = frame_id_base + "_optical_link"; //------------------- // Published topics //------------------- - this->cloud_pub_ = this->np_.advertise("cloud", 1); - this->distance_pub_ = this->it_->advertise("distance", 1); - this->distance_noise_pub_ = this->it_->advertise("distance_noise", 1); - this->amplitude_pub_ = this->it_->advertise("amplitude", 1); - this->raw_amplitude_pub_ = this->it_->advertise("raw_amplitude", 1); - this->conf_pub_ = this->it_->advertise("confidence", 1); - this->gray_image_pub_ = this->it_->advertise("gray_image", 1); - this->rgb_image_pub_ = this->np_.advertise("rgb_image/compressed", 1); - - // we latch the unit vectors - this->uvec_pub_ = this->np_.advertise("unit_vectors", 1, true); - - this->extrinsics_pub_ = this->np_.advertise("extrinsics", 1); + if (strcmp(this->imager_type_.c_str(), "3D") == 0 && this->xyz_image_stream_) + { + this->cloud_pub_ = this->np_.advertise("cloud", 1); + NODELET_DEBUG_STREAM("Point cloud publisher active"); + } + + if (strcmp(this->imager_type_.c_str(), "3D") == 0 && this->radial_distance_image_stream_) + { + this->distance_pub_ = this->it_->advertise("distance", 1); + NODELET_DEBUG_STREAM("Distance image publisher active"); + } + + if (strcmp(this->imager_type_.c_str(), "3D") == 0 && this->radial_distance_noise_stream_) + { + this->distance_noise_pub_ = this->it_->advertise("distance_noise", 1); + NODELET_DEBUG_STREAM("Distance noise image publisher active"); + } + + if (strcmp(this->imager_type_.c_str(), "3D") == 0 && this->amplitude_image_stream_) + { + this->amplitude_pub_ = this->it_->advertise("amplitude", 1); + NODELET_DEBUG_STREAM("Amplitude image publisher active"); + } + + if (strcmp(this->imager_type_.c_str(), "3D") == 0 && this->confidence_image_stream_) + { + this->conf_pub_ = this->it_->advertise("confidence", 1); + NODELET_DEBUG_STREAM("Confidence image publisher active"); + } + + if (strcmp(this->imager_type_.c_str(), "2D") == 0 && this->rgb_image_stream_) + { + this->rgb_image_pub_ = this->np_.advertise("rgb_image/compressed", 1); + NODELET_DEBUG_STREAM("2D RGB image publisher active"); + } + + if (this->extrinsic_image_stream_) + { + this->extrinsics_pub_ = this->np_.advertise("extrinsics", 1); + NODELET_DEBUG_STREAM("Extrinsics parameter publisher active"); + } + + // if (this->intrinsic_image_stream) + // { + // this->intrinsics_pub_ = this->np_.advertise("intrinsics", 1); + // } NODELET_DEBUG_STREAM("after advertising the publishers"); + + + //--------------------- // Advertised Services //--------------------- this->dump_srv_ = this->np_.advertiseService( "Dump", std::bind(&CameraNodelet::Dump, this, std::placeholders::_1, std::placeholders::_2)); + this->dump_json_schema_srv_ = this->np_.advertiseService( + "DumpJSONSchema", std::bind(&CameraNodelet::DumpJSONSchema, this, std::placeholders::_1, std::placeholders::_2)); + this->config_srv_ = this->np_.advertiseService( "Config", std::bind(&CameraNodelet::Config, this, std::placeholders::_1, std::placeholders::_2)); @@ -284,6 +415,8 @@ void ifm3d_ros::CameraNodelet::onInit() "SoftOn", std::bind(&CameraNodelet::SoftOn, this, std::placeholders::_1, std::placeholders::_2)); NODELET_DEBUG_STREAM("after advertise service"); + ros::Duration(5.0).sleep(); + //---------------------------------- // Fire off our main publishing loop //---------------------------------- @@ -302,7 +435,42 @@ bool ifm3d_ros::CameraNodelet::Dump(ifm3d_ros_msgs::Dump::Request& req, ifm3d_ro json j = this->cam_->ToJSON(); res.config = j.dump(); } - catch (const ifm3d::error_t& ex) + catch (const ifm3d::Error& ex) + { + res.status = ex.code(); + NODELET_WARN_STREAM(ex.what()); + } + catch (const std::exception& std_ex) + { + res.status = -1; + NODELET_WARN_STREAM(std_ex.what()); + } + catch (...) + { + res.status = -2; + } + + if (res.status != 0) + { + NODELET_WARN_STREAM("Dump: " << res.status); + } + + return true; +} + + +bool ifm3d_ros::CameraNodelet::DumpJSONSchema(ifm3d_ros_msgs::DumpJSONSchema::Request& req, ifm3d_ros_msgs::DumpJSONSchema::Response& res) +{ + std::lock_guard lock(this->mutex_); + res.status = 0; + + try + { + ifm3d::O3R::Ptr cam_O3R = std::static_pointer_cast(this->cam_); + json j = cam_O3R->GetSchema(); + res.config = j.dump(); + } + catch (const ifm3d::Error& ex) { res.status = ex.code(); NODELET_WARN_STREAM(ex.what()); @@ -325,6 +493,7 @@ bool ifm3d_ros::CameraNodelet::Dump(ifm3d_ros_msgs::Dump::Request& req, ifm3d_ro return true; } + bool ifm3d_ros::CameraNodelet::Config(ifm3d_ros_msgs::Config::Request& req, ifm3d_ros_msgs::Config::Response& res) { std::lock_guard lock(this->mutex_); @@ -335,7 +504,7 @@ bool ifm3d_ros::CameraNodelet::Config(ifm3d_ros_msgs::Config::Request& req, ifm3 { this->cam_->FromJSON(json::parse(req.json)); } - catch (const ifm3d::error_t& ex) + catch (const ifm3d::Error& ex) { res.status = ex.code(); res.msg = ex.what(); @@ -369,7 +538,7 @@ bool ifm3d_ros::CameraNodelet::Trigger(ifm3d_ros_msgs::Trigger::Request& req, if { this->fg_->SWTrigger(); } - catch (const ifm3d::error_t& ex) + catch (const ifm3d::Error& ex) { res.status = ex.code(); } @@ -378,8 +547,7 @@ bool ifm3d_ros::CameraNodelet::Trigger(ifm3d_ros_msgs::Trigger::Request& req, if return true; } -// this is a dummy method for the moment: the idea of applications is not supported for the O3RCamera -// we keep this in to possibly keep it comparable / interoperable with the ROS wrappers for other ifm cameras + bool ifm3d_ros::CameraNodelet::SoftOff(ifm3d_ros_msgs::SoftOff::Request& req, ifm3d_ros_msgs::SoftOff::Response& res) { std::lock_guard lock(this->mutex_); @@ -392,27 +560,26 @@ bool ifm3d_ros::CameraNodelet::SoftOff(ifm3d_ros_msgs::SoftOff::Request& req, if port_arg = static_cast(this->pcic_port_) % 50010; // Configure the device from a json string - this->cam_->FromJSONStr("{\"ports\":{\"port" + std::to_string(port_arg) + "\": {\"state\": \"IDLE\"}}}"); + this->cam_->FromJSONStr("{\"ports\":{\"port" + std::to_string(port_arg) + "\": {\"state\": \"CONF\"}}}"); this->assume_sw_triggered_ = false; this->timeout_millis_ = this->soft_on_timeout_millis_; this->timeout_tolerance_secs_ = this->soft_on_timeout_tolerance_secs_; } - catch (const ifm3d::error_t& ex) + catch (const ifm3d::Error& ex) { res.status = ex.code(); res.msg = ex.what(); return false; } - NODELET_WARN_STREAM("The concept of applications is not available for the O3R - we use IDLE and RUN states instead"); - res.msg = "{\"ports\":{\"port" + std::to_string(port_arg) + "\": {\"state\": \"IDLE\"}}}"; + NODELET_INFO_STREAM("Switched state to CONF"); + res.msg = "{\"ports\":{\"port" + std::to_string(port_arg) + "\": {\"state\": \"CONF\"}}}"; return true; } -// this is a dummy method for the moment: the idea of applications is not supported for the O3RCamera -// we keep this in to possibly keep it comparable / interoperable with the ROS wrappers for other ifm cameras + bool ifm3d_ros::CameraNodelet::SoftOn(ifm3d_ros_msgs::SoftOn::Request& req, ifm3d_ros_msgs::SoftOn::Response& res) { std::lock_guard lock(this->mutex_); @@ -423,22 +590,6 @@ bool ifm3d_ros::CameraNodelet::SoftOn(ifm3d_ros_msgs::SoftOn::Request& req, ifm3 { port_arg = static_cast(this->pcic_port_) % 50010; - // try getting a current configuration as an ifm3d dump - // this way a a-priori test before setting the state can be tested - // try - // { - // json j = this->cam_->ToJSON(); - // } - // catch (const ifm3d::error_t& ex) - // { - // NODELET_WARN_STREAM(ex.code()); - // NODELET_WARN_STREAM(ex.what()); - // } - // catch (const std::exception& std_ex) - // { - // NODELET_WARN_STREAM(std_ex.what()); - // } - // Configure the device from a json string this->cam_->FromJSONStr("{\"ports\":{\"port" + std::to_string(port_arg) + "\": {\"state\": \"RUN\"}}}"); @@ -446,48 +597,60 @@ bool ifm3d_ros::CameraNodelet::SoftOn(ifm3d_ros_msgs::SoftOn::Request& req, ifm3 this->timeout_millis_ = this->soft_on_timeout_millis_; this->timeout_tolerance_secs_ = this->soft_on_timeout_tolerance_secs_; } - catch (const ifm3d::error_t& ex) + catch (const ifm3d::Error& ex) { res.status = ex.code(); res.msg = ex.what(); return false; } - NODELET_WARN_STREAM("The concept of applications is not available for the O3R - we use IDLE and RUN states instead"); + NODELET_INFO_STREAM("Switched state to RUN"); res.msg = "{\"ports\":{\"port" + std::to_string(port_arg) + "\": {\"state\": \"RUN\"}}}"; return true; } -bool ifm3d_ros::CameraNodelet::InitStructures(std::uint16_t mask, std::uint16_t pcic_port) +std::string ifm3d_ros::CameraNodelet::GetCameraType(std::uint16_t pcic_port) +{ + std::lock_guard lock(this->mutex_); + + auto cam_O3R = std::make_shared(this->camera_ip_, this->xmlrpc_port_); + std::vector ports_vector_ = cam_O3R->Ports(); + + int port_arg = static_cast(this->pcic_port_) % 50010; + std::string port_type_ = cam_O3R->Port("port" + std::to_string(port_arg)).type; + NODELET_INFO_ONCE("Imager type as retrieved from API device info: PCIC port %d, type %s", (int) this->pcic_port_, port_type_.c_str()); + + return port_type_; +} + +bool ifm3d_ros::CameraNodelet::InitStructures(std::uint16_t pcic_port) { std::lock_guard lock(this->mutex_); bool retval = false; + int port_arg = -1; + try { NODELET_INFO_STREAM("Running dtors..."); - this->im_.reset(); this->fg_.reset(); this->cam_.reset(); NODELET_INFO_STREAM("Initializing camera..."); - this->cam_ = ifm3d::CameraBase::MakeShared(this->camera_ip_, this->xmlrpc_port_); - ros::Duration(1.0).sleep(); + this->cam_ = ifm3d::Device::MakeShared(this->camera_ip_, this->xmlrpc_port_); + ros::Duration(5.0).sleep(); NODELET_INFO_STREAM("Initializing framegrabber..."); - this->fg_ = std::make_shared(this->cam_, mask, this->pcic_port_); - NODELET_INFO("Nodelet arguments: %d, %d", (int)mask, (int)this->pcic_port_); - - NODELET_INFO_STREAM("Initializing image buffer..."); - this->im_ = std::make_shared(); + this->fg_ = std::make_shared(this->cam_, this->pcic_port_); + NODELET_INFO_ONCE("Nodelet argument: %d", (int)this->pcic_port_); + ros::Duration(5.0).sleep(); retval = true; } - catch (const ifm3d::error_t& ex) + catch (const ifm3d::Error& ex) { NODELET_WARN_STREAM(ex.code() << ": " << ex.what()); - this->im_.reset(); this->fg_.reset(); this->cam_.reset(); retval = false; @@ -496,147 +659,94 @@ bool ifm3d_ros::CameraNodelet::InitStructures(std::uint16_t mask, std::uint16_t return retval; } -// this is the helper function for retrieving complete pcic frames -bool ifm3d_ros::CameraNodelet::AcquireFrame() -{ - std::lock_guard lock(this->mutex_); - bool retval = false; - NODELET_DEBUG_STREAM("try receiving data via fg WaitForFrame"); - try - { - retval = this->fg_->WaitForFrame(this->im_.get(), this->timeout_millis_); - } - catch (const ifm3d::error_t& ex) - { - NODELET_WARN_STREAM(ex.code() << ": " << ex.what()); - retval = false; - } - - return retval; -} - -void ifm3d_ros::CameraNodelet::Run() -{ - std::unique_lock lock(this->mutex_, std::defer_lock); - - NODELET_DEBUG_STREAM("in Run"); - - // We need to account for the case of when the nodelet is being started prior - // to the camera being plugged in. - - while (ros::ok() && (!this->InitStructures(ifm3d::IMG_UVEC, this->pcic_port_))) - { - NODELET_WARN_STREAM("Could not initialize pixel stream!"); - ros::Duration(1.0).sleep(); - } - - ifm3d::Image confidence_img; - ifm3d::Image distance_img; - ifm3d::Image distance_noise_img; - ifm3d::Image amplitude_img; - ifm3d::Image xyz_img; - ifm3d::Image raw_amplitude_img; - ifm3d::Image gray_img; - ifm3d::Image rgb_img; +void ifm3d_ros::CameraNodelet::Callback2D(ifm3d::Frame::Ptr frame){ + // + // Pull out all the wrapped images so that we can release the "GIL" + // while publishing + // + std::unique_lock lock(this->mutex_, std::defer_lock); + lock.lock(); + ifm3d::Buffer extrinsics; + ifm3d::Buffer rgb_img; + this->last_frame_local_time_ = ros::Time::now(); + // ifm3d::Buffer rgb_img_info; - NODELET_DEBUG_STREAM("after initializing the opencv buffers"); - std::vector extrinsics(6); - // XXX: need to implement a nice strategy for getting the actual times - // from the camera which are registered to the frame data in the image - // buffer. - ros::Time last_frame = ros::Time::now(); - bool got_uvec = false; + NODELET_DEBUG_STREAM("start getting data"); + try + { + rgb_img = frame->GetBuffer(ifm3d::buffer_id::JPEG_IMAGE); + // extrinsics = frame->GetBuffer(ifm3d::buffer_id::EXTRINSIC_CALIB); + // rgb_img_info = frame->GetBuffer(ifm3d::buffer_id::RGB_INFO); + // this->last_frame_time_ = frame->TimeStamps()[0]; - while (ros::ok()) - { - if (!this->AcquireFrame()) + } + catch (const ifm3d::Error& ex) { - if (!this->assume_sw_triggered_) - { - NODELET_WARN_STREAM("Timeout waiting for camera!"); - } - else - { - ros::Duration(.001).sleep(); - } + NODELET_WARN_STREAM(ex.what()); + } + catch (const std::exception& std_ex) + { + NODELET_WARN_STREAM(std_ex.what()); + } + NODELET_DEBUG_STREAM("finished getting data"); - if ((ros::Time::now() - last_frame).toSec() > this->timeout_tolerance_secs_) - { - NODELET_WARN_STREAM("Attempting to restart framegrabber..."); - while (!this->InitStructures(got_uvec ? this->schema_mask_ : ifm3d::IMG_UVEC, this->pcic_port_)) - { - NODELET_WARN_STREAM("Could not re-initialize pixel stream!"); - ros::Duration(1.0).sleep(); - } + lock.unlock(); - last_frame = ros::Time::now(); - } + // + // Now, do the publishing + // - continue; + // Timestamps: + this->head.stamp = ros::Time( + std::chrono::duration_cast>>(frame->TimeStamps()[0].time_since_epoch()).count() + ); + if ((ros::Time::now() - this->head.stamp) > ros::Duration(this->frame_latency_thresh_)) + { + NODELET_INFO_STREAM("Camera's time and client's time are not synced"); + this->head.stamp = ros::Time::now(); } + // Header frame_id + this->head.frame_id = this->frame_id_; - last_frame = ros::Time::now(); - - NODELET_DEBUG_STREAM("prepare header"); - std_msgs::Header head = std_msgs::Header(); - head.frame_id = this->frame_id_; - head.stamp = ros::Time(std::chrono::duration_cast>>( - this->im_->TimeStamp().time_since_epoch()) - .count()); - if ((ros::Time::now() - head.stamp) > ros::Duration(this->frame_latency_thresh_)) + if (this->rgb_image_stream_ && frame->HasBuffer(ifm3d::buffer_id::JPEG_IMAGE)) { - NODELET_INFO_ONCE("Camera's time and client's time are not synced"); - head.stamp = ros::Time::now(); + this->rgb_image_pub_.publish(ifm3d_to_ros_compressed_image(rgb_img, optical_head, "jpeg", getName())); + NODELET_DEBUG_STREAM("after publishing rgb image"); } - NODELET_DEBUG_STREAM("in header, before setting header to msgs"); - std_msgs::Header optical_head = std_msgs::Header(); - optical_head.stamp = head.stamp; - optical_head.frame_id = this->optical_frame_id_; - - // currently the unit vector calculation seems to be missing in the ifm3d state: therefore we don't publish anything - // to the uvec pubisher publish unit vectors once on a latched topic, then re-initialize the framegrabber with the - // user's requested schema mask - if (!got_uvec) - { - lock.lock(); - sensor_msgs::Image uvec_msg = ifm3d_to_ros_image(this->im_->UnitVectors(), optical_head, getName()); - NODELET_INFO_STREAM("uvec image size: " << uvec_msg.height * uvec_msg.width); - lock.unlock(); - this->uvec_pub_.publish(uvec_msg); - got_uvec = true; - NODELET_INFO("Got unit vectors, restarting framegrabber with mask: %d", (int)this->schema_mask_); - - while (!this->InitStructures(this->schema_mask_, this->pcic_port_)) - { - NODELET_WARN("Could not re-initialize pixel stream!"); - ros::Duration(1.0).sleep(); - } - NODELET_INFO_STREAM("Start streaming data"); - continue; - } +} +void ifm3d_ros::CameraNodelet::Callback3D(ifm3d::Frame::Ptr frame){ // // Pull out all the wrapped images so that we can release the "GIL" // while publishing // + std::unique_lock lock(this->mutex_, std::defer_lock); lock.lock(); + ifm3d::Buffer xyz_img; + ifm3d::Buffer confidence_img; + ifm3d::Buffer distance_img; + ifm3d::Buffer distance_noise_img; + ifm3d::Buffer amplitude_img; + ifm3d::Buffer extrinsics; + ifm3d::Buffer rgb_img; NODELET_DEBUG_STREAM("start getting data"); try { - xyz_img = this->im_->XYZImage(); - confidence_img = this->im_->ConfidenceImage(); - distance_img = this->im_->DistanceImage(); - distance_noise_img = this->im_->DistanceNoiseImage(); - amplitude_img = this->im_->AmplitudeImage(); - raw_amplitude_img = this->im_->RawAmplitudeImage(); - gray_img = this->im_->GrayImage(); - extrinsics = this->im_->Extrinsics(); - rgb_img = this->im_->JPEGImage(); + xyz_img =frame->GetBuffer(ifm3d::buffer_id::XYZ); + confidence_img = frame->GetBuffer(ifm3d::buffer_id::CONFIDENCE_IMAGE); + distance_img = frame->GetBuffer(ifm3d::buffer_id::RADIAL_DISTANCE_IMAGE); + distance_noise_img = frame->GetBuffer(ifm3d::buffer_id::RADIAL_DISTANCE_NOISE); + amplitude_img = frame->GetBuffer(ifm3d::buffer_id::NORM_AMPLITUDE_IMAGE); + extrinsics = frame->GetBuffer(ifm3d::buffer_id::EXTRINSIC_CALIB); + this->last_frame_time_ = frame->TimeStamps()[0]; + this->last_frame_local_time_ = ros::Time::now(); + } - catch (const ifm3d::error_t& ex) + catch (const ifm3d::Error& ex) { NODELET_WARN_STREAM(ex.what()); } @@ -652,79 +762,160 @@ void ifm3d_ros::CameraNodelet::Run() // Now, do the publishing // - NODELET_DEBUG_STREAM("start publishing"); - // Confidence image is invariant - no need to check the mask - this->conf_pub_.publish(ifm3d_to_ros_image(confidence_img, optical_head, getName())); - NODELET_DEBUG_STREAM("after publishing confidence image"); + // Timestamps: + this->head.stamp = ros::Time( + std::chrono::duration_cast>>(frame->TimeStamps()[0].time_since_epoch()).count() + ); + if ((ros::Time::now() - this->head.stamp) > ros::Duration(this->frame_latency_thresh_)) + { + NODELET_INFO_ONCE("Camera's time and client's time are not synced"); + this->head.stamp = ros::Time::now(); + } + // Header frame_id + this->head.frame_id = this->frame_id_; + + if (this->amplitude_image_stream_ && frame->HasBuffer(ifm3d::buffer_id::NORM_AMPLITUDE_IMAGE)) + { + this->amplitude_pub_.publish(ifm3d_to_ros_image(amplitude_img, optical_head, getName())); + NODELET_DEBUG_STREAM("after publishing norm amplitude image"); + } - if ((this->schema_mask_ & ifm3d::IMG_CART) == ifm3d::IMG_CART) + if (this->confidence_image_stream_ && frame->HasBuffer(ifm3d::buffer_id::CONFIDENCE_IMAGE)) { - this->cloud_pub_.publish(ifm3d_to_ros_cloud(xyz_img, head, getName())); - NODELET_DEBUG_STREAM("after publishing xyz image"); + this->conf_pub_.publish(ifm3d_to_ros_image(confidence_img, optical_head, getName())); + NODELET_DEBUG_STREAM("after publishing confidence image"); } - if ((this->schema_mask_ & ifm3d::IMG_RDIS) == ifm3d::IMG_RDIS) + if (this->radial_distance_image_stream_ && frame->HasBuffer(ifm3d::buffer_id::RADIAL_DISTANCE_IMAGE)) { this->distance_pub_.publish(ifm3d_to_ros_image(distance_img, optical_head, getName())); NODELET_DEBUG_STREAM("after publishing distance image"); } - if ((this->schema_mask_ & ifm3d::IMG_DIS_NOISE) == ifm3d::IMG_DIS_NOISE) + if (this->radial_distance_noise_stream_ && frame->HasBuffer(ifm3d::buffer_id::RADIAL_DISTANCE_NOISE)) { this->distance_noise_pub_.publish(ifm3d_to_ros_image(distance_noise_img, optical_head, getName())); NODELET_DEBUG_STREAM("after publishing distance noise image"); } - if ((this->schema_mask_ & ifm3d::IMG_AMP) == ifm3d::IMG_AMP) + if (this->xyz_image_stream_ && frame->HasBuffer(ifm3d::buffer_id::XYZ)) { - this->amplitude_pub_.publish(ifm3d_to_ros_image(amplitude_img, optical_head, getName())); - NODELET_DEBUG_STREAM("after publishing amplitude image"); + this->cloud_pub_.publish(ifm3d_to_ros_cloud(xyz_img, head, getName())); + NODELET_DEBUG_STREAM("after publishing point cloud image"); } - if ((this->schema_mask_ & ifm3d::IMG_RAMP) == ifm3d::IMG_RAMP) + + // + // publish extrinsics + // + if (this->extrinsic_image_stream_ && frame->HasBuffer(ifm3d::buffer_id::EXTRINSIC_CALIB)) { - this->raw_amplitude_pub_.publish(ifm3d_to_ros_image(raw_amplitude_img, optical_head, getName())); - NODELET_DEBUG_STREAM("Raw amplitude image publisher is a dummy publisher - data will be added soon"); - NODELET_DEBUG_STREAM("after publishing raw amplitude image"); + NODELET_DEBUG_STREAM("start publishing extrinsics"); + ifm3d_ros_msgs::Extrinsics extrinsics_msg; + extrinsics_msg.header = optical_head; + try + { + ifm3d::Buffer_ ext = extrinsics; + extrinsics_msg.tx = ext.at(0); + extrinsics_msg.ty = ext.at(1); + extrinsics_msg.tz = ext.at(2); + extrinsics_msg.rot_x = ext.at(3); + extrinsics_msg.rot_y = ext.at(4); + extrinsics_msg.rot_z = ext.at(5); + } + catch (const std::out_of_range& ex) + { + NODELET_WARN("out-of-range error fetching extrinsics"); + } + this->extrinsics_pub_.publish(extrinsics_msg); } +} - if ((this->schema_mask_ & ifm3d::IMG_GRAY) == ifm3d::IMG_GRAY) +// this is the helper function for retrieving complete pcic frames +bool ifm3d_ros::CameraNodelet::StartStream() +{ + bool retval = false; + NODELET_INFO_STREAM("Start streaming frames"); + try + { + // need to implement a nice strategy for getting the actual times + // from the camera which are registered to the frame data in the image + // buffer. + + if (strcmp(this->imager_type_.c_str(), "3D") == 0) { - this->gray_image_pub_.publish(ifm3d_to_ros_image(gray_img, optical_head, getName())); - NODELET_DEBUG_STREAM("Gray image publisher is a dummy publisher - data will be added soon"); - NODELET_DEBUG_STREAM("after publishing gray image"); + fg_->Start(this->schema_mask_default_3d_); + NODELET_DEBUG_STREAM("Framegabbber initialized with default 3D schema mask"); + fg_->OnNewFrame(std::bind(&ifm3d_ros::CameraNodelet::Callback3D, this, std::placeholders::_1)); } - // The 2D is not yet settable in the schema mask: publish all the time + else if (strcmp(this->imager_type_.c_str(), "2D") == 0) + { + fg_->Start(this->schema_mask_default_2d_); + NODELET_DEBUG_STREAM("Framegabbber initialized with default 2D schema mask"); + fg_->OnNewFrame(std::bind(&ifm3d_ros::CameraNodelet::Callback2D, this, std::placeholders::_1)); + } - if (rgb_img.height() * rgb_img.width() > 0) + else { - this->rgb_image_pub_.publish(ifm3d_to_ros_compressed_image(rgb_img, optical_head, "jpeg", getName())); - NODELET_DEBUG_STREAM("after publishing rgb image"); + NODELET_INFO_STREAM("Unknown imager type"); } + this->last_frame_local_time_ = ros::Time::now(); - // - // publish extrinsics - // - NODELET_DEBUG_STREAM("start publishing extrinsics"); - ifm3d_ros_msgs::Extrinsics extrinsics_msg; - extrinsics_msg.header = optical_head; - try + } + catch (const ifm3d::Error& ex) + { + NODELET_WARN_STREAM(ex.code() << ": " << ex.what()); + retval = false; + } + + return retval; +} + +void ifm3d_ros::CameraNodelet::Run() +{ + std::unique_lock lock(this->mutex_, std::defer_lock); + + NODELET_DEBUG_STREAM("in CameraNodelet Run"); + ros::Duration(5.0).sleep(); + + while (ros::ok() && (!this->InitStructures(this->pcic_port_))) + { + NODELET_WARN_STREAM("Could not initialize pixel stream! Re-Initializing"); + ros::Duration(this->timeout_tolerance_secs_).sleep(); + } + + NODELET_DEBUG_STREAM("after initializing the buffers and services"); + + this->StartStream(); + NODELET_INFO_STREAM("Started the camera stream"); + + while (ros::ok()) + { + if ((ros::Time::now() - last_frame_local_time_).toSec() > this->timeout_tolerance_secs_) { - extrinsics_msg.tx = extrinsics.at(0); - extrinsics_msg.ty = extrinsics.at(1); - extrinsics_msg.tz = extrinsics.at(2); - extrinsics_msg.rot_x = extrinsics.at(3); - extrinsics_msg.rot_y = extrinsics.at(4); - extrinsics_msg.rot_z = extrinsics.at(5); + if (!this->assume_sw_triggered_) + { + NODELET_WARN_STREAM("Timeout waiting for camera!"); + NODELET_WARN_STREAM("Attempting to restart framegrabber..."); + while (!this->InitStructures(this->pcic_port_)) + { + NODELET_WARN_STREAM("Could not re-initialize pixel stream!"); + ros::Duration(1.0).sleep(); + } + this->StartStream(); + } } - catch (const std::out_of_range& ex) + else { - NODELET_WARN("out-of-range error fetching extrinsics"); + ros::Duration(.001).sleep(); } - this->extrinsics_pub_.publish(extrinsics_msg); - } // end: while (ros::ok()) { ... } + continue; + } + + fg_->Stop(); } // end: Run() PLUGINLIB_EXPORT_CLASS(ifm3d_ros::CameraNodelet, nodelet::Nodelet) diff --git a/ifm3d_ros_examples/launch/camera_2d.launch b/ifm3d_ros_examples/launch/camera_2d.launch new file mode 100644 index 0000000..f72601a --- /dev/null +++ b/ifm3d_ros_examples/launch/camera_2d.launch @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + " + " + " + " + " + " + " + + + diff --git a/ifm3d_ros_examples/launch/camera.launch b/ifm3d_ros_examples/launch/camera_3d.launch similarity index 51% rename from ifm3d_ros_examples/launch/camera.launch rename to ifm3d_ros_examples/launch/camera_3d.launch index 78ae71e..020fbe4 100644 --- a/ifm3d_ros_examples/launch/camera.launch +++ b/ifm3d_ros_examples/launch/camera_3d.launch @@ -1,31 +1,46 @@ - + - + + + + + + + + - + - + + + " + " + " + " + " + " + " diff --git a/ifm3d_ros_examples/launch/dump_merge_pc.json b/ifm3d_ros_examples/launch/dump_merge_pc.json deleted file mode 100644 index 2e8fa74..0000000 --- a/ifm3d_ros_examples/launch/dump_merge_pc.json +++ /dev/null @@ -1,338 +0,0 @@ -{ - "device": { - "clock": { - "currentTime": 1581090835576664320 - }, - "diagnostic": { - "temperatures": [], - "upTime": 190000000000 - }, - "info": { - "device": "0301", - "deviceTreeBinaryBlob": "tegra186-quill-p3310-1000-c03-00-base.dtb", - "features": {}, - "name": "", - "partNumber": "M03903", - "productionState": "AA", - "serialNumber": "000201233338", - "vendor": "0001" - }, - "network": { - "authorized_keys": "", - "ipAddressConfig": 0, - "macEth0": "00:04:4B:E4:DA:9E", - "macEth1": "00:02:01:23:33:38", - "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+g231628d3fa15", - "l4t": "r32.4.3", - "os": "0.13.3-188", - "schema": "feature/O3R-4949", - "swu": "0.15.3" - } - }, - "ports": { - "port0": { - "acquisition": { - "exposureLong": 5000, - "exposureShort": 400, - "framerate": 10.0, - "offset": 0.0, - "version": { - "major": 0, - "minor": 0, - "patch": 0 - } - }, - "data": { - "algoDebugConfig": {}, - "availablePCICOutput": [ - "AMPLITUDE_COMPRESSED", - "CONFIDENCE", - "RADIAL_DISTANCE_COMPRESSED", - "RADIAL_DISTANCE_NOISE", - "REFLECTIVITY", - "TOF_INFO" - ], - "pcicTCPPort": 50010 - }, - "info": { - "device": "3101", - "deviceTreeBinaryBlobOverlay": "001-irs2381c.dtbo", - "features": { - "fov": { - "horizontal": 78, - "vertical": 105 - }, - "resolution": { - "height": 172, - "width": 224 - }, - "type": "3D" - }, - "name": "", - "partNumber": "M03957", - "productionState": "AA", - "sensor": "IRS2381C", - "sensorID": "IRS2381C_105x78_4x2W_110x90_Csample", - "serialNumber": "000000000186", - "vendor": "0001" - }, - "mode": "experimental_high_4m", - "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.0, - "minReflectivity": 0.0, - "mixedPixelFilterMode": 1, - "mixedPixelThresholdRad": 0.15 - }, - "extrinsicHeadToUser": { - "rotX": 0.0, - "rotY": -1.5708, - "rotZ": 0.0, - "transX": 0.015, - "transY": -0.18, - "transZ": -0.055 - }, - "version": { - "major": 0, - "minor": 0, - "patch": 0 - } - }, - "state": "RUN" - }, - "port1": { - "acquisition": { - "exposureLong": 5000, - "exposureShort": 400, - "framerate": 10.0, - "offset": 0.0, - "version": { - "major": 0, - "minor": 0, - "patch": 0 - } - }, - "data": { - "algoDebugConfig": {}, - "availablePCICOutput": [ - "AMPLITUDE_COMPRESSED", - "CONFIDENCE", - "RADIAL_DISTANCE_COMPRESSED", - "RADIAL_DISTANCE_NOISE", - "REFLECTIVITY", - "TOF_INFO" - ], - "pcicTCPPort": 50011 - }, - "info": { - "device": "3101", - "deviceTreeBinaryBlobOverlay": "001-irs2381c.dtbo", - "features": { - "fov": { - "horizontal": 78, - "vertical": 105 - }, - "resolution": { - "height": 172, - "width": 224 - }, - "type": "3D" - }, - "name": "", - "partNumber": "M03957", - "productionState": "AA", - "sensor": "IRS2381C", - "sensorID": "IRS2381C_105x78_4x2W_110x90_Csample", - "serialNumber": "000000000192", - "vendor": "0001" - }, - "mode": "experimental_high_4m", - "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.0, - "minReflectivity": 0.0, - "mixedPixelFilterMode": 1, - "mixedPixelThresholdRad": 0.15 - }, - "extrinsicHeadToUser": { - "rotX": 0.0, - "rotY": 0.0, - "rotZ": 0.0, - "transX": -0.13, - "transY": -0.1, - "transZ": 0.2 - }, - "version": { - "major": 0, - "minor": 0, - "patch": 0 - } - }, - "state": "RUN" - }, - "port2": { - "acquisition": { - "exposureLong": 5000, - "exposureShort": 400, - "framerate": 10.0, - "offset": 0.0, - "version": { - "major": 0, - "minor": 0, - "patch": 0 - } - }, - "data": { - "algoDebugConfig": {}, - "availablePCICOutput": [ - "AMPLITUDE_COMPRESSED", - "CONFIDENCE", - "RADIAL_DISTANCE_COMPRESSED", - "RADIAL_DISTANCE_NOISE", - "REFLECTIVITY", - "TOF_INFO" - ], - "pcicTCPPort": 50012 - }, - "info": { - "device": "2301", - "deviceTreeBinaryBlobOverlay": "001-irs2381c.dtbo", - "features": { - "fov": { - "horizontal": 80, - "vertical": 127 - }, - "resolution": { - "height": 800, - "width": 1280 - }, - "type": "2D" - }, - "name": "", - "partNumber": "M03934", - "productionState": "AA", - "sensor": "OV9782", - "sensorID": "OV9782_127x80_noIllu_Csample", - "serialNumber": "000000000094", - "vendor": "0001" - }, - "mode": "experimental_high_4m", - "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.0, - "minReflectivity": 0.0, - "mixedPixelFilterMode": 1, - "mixedPixelThresholdRad": 0.15 - }, - "extrinsicHeadToUser": { - "rotX": 0.0, - "rotY": 3.1416, - "rotZ": 0.0, - "transX": -0.015, - "transY": -0.3, - "transZ": 0.05 - }, - "version": { - "major": 0, - "minor": 0, - "patch": 0 - } - }, - "state": "RUN" - }, - "port5": { - "acquisition": { - "framerate": 10.0, - "version": { - "major": 0, - "minor": 0, - "patch": 0 - } - }, - "data": { - "algoDebugConfig": {}, - "availablePCICOutput": [ - "RGB_INFO" - ], - "pcicTCPPort": 50015 - }, - "info": { - "device": "2301", - "deviceTreeBinaryBlobOverlay": "001-ov9782.dtbo", - "features": { - "fov": { - "horizontal": 80, - "vertical": 127 - }, - "resolution": { - "height": 800, - "width": 1280 - }, - "type": "2D" - }, - "name": "", - "partNumber": "M03934", - "productionState": "AA", - "sensor": "OV9782", - "sensorID": "OV9782_127x80_noIllu_Csample", - "serialNumber": "000000000094", - "vendor": "0001" - }, - "mode": "experimental_autoexposure2D", - "processing": { - "extrinsicHeadToUser": { - "rotX": 0.0, - "rotY": 0.0, - "rotZ": 0.0, - "transX": 0.0, - "transY": 0.0, - "transZ": 0.0 - }, - "version": { - "major": 0, - "minor": 0, - "patch": 0 - } - }, - "state": "CONF" - } - } -} diff --git a/ifm3d_ros_examples/launch/four_cameras.launch b/ifm3d_ros_examples/launch/four_cameras.launch deleted file mode 100644 index 6ee2d6f..0000000 --- a/ifm3d_ros_examples/launch/four_cameras.launch +++ /dev/null @@ -1,110 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ifm3d_ros_examples/launch/head.launch b/ifm3d_ros_examples/launch/head.launch deleted file mode 100644 index fc45542..0000000 --- a/ifm3d_ros_examples/launch/head.launch +++ /dev/null @@ -1,60 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ifm3d_ros_examples/launch/merge_pc.launch b/ifm3d_ros_examples/launch/merge_pc.launch deleted file mode 100644 index 13bf287..0000000 --- a/ifm3d_ros_examples/launch/merge_pc.launch +++ /dev/null @@ -1,147 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ifm3d_ros_examples/launch/nodelet.launch b/ifm3d_ros_examples/launch/nodelet.launch index 8259ed6..4929ff6 100644 --- a/ifm3d_ros_examples/launch/nodelet.launch +++ b/ifm3d_ros_examples/launch/nodelet.launch @@ -7,12 +7,20 @@ - + - + + + + + + + + + @@ -105,6 +154,17 @@ + + + + " + " + " + " + " + " + " + diff --git a/ifm3d_ros_examples/launch/rviz.launch b/ifm3d_ros_examples/launch/rviz.launch deleted file mode 100644 index f0a26ed..0000000 --- a/ifm3d_ros_examples/launch/rviz.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - diff --git a/ifm3d_ros_examples/launch/six_cameras.launch b/ifm3d_ros_examples/launch/six_cameras.launch deleted file mode 100644 index b8a2807..0000000 --- a/ifm3d_ros_examples/launch/six_cameras.launch +++ /dev/null @@ -1,156 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ifm3d_ros_examples/package.xml b/ifm3d_ros_examples/package.xml index f8db7eb..3f6d8e6 100644 --- a/ifm3d_ros_examples/package.xml +++ b/ifm3d_ros_examples/package.xml @@ -2,11 +2,11 @@ ifm3d_ros_examples - 0.1.0 + 0.2.0 ifm3d_ros examples subpackage - ifm CSR group + ifm robotics group Apache 2 - CSR ifm sytron + ifm robotics group https://github.com/ifm/ifm3d_ros/ diff --git a/ifm3d_ros_msgs/CMakeLists.txt b/ifm3d_ros_msgs/CMakeLists.txt index df3e3f0..0372eae 100644 --- a/ifm3d_ros_msgs/CMakeLists.txt +++ b/ifm3d_ros_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ add_service_files( DIRECTORY srv FILES Dump.srv + DumpJSONSchema.srv Config.srv Trigger.srv SoftOff.srv diff --git a/ifm3d_ros_msgs/package.xml b/ifm3d_ros_msgs/package.xml index 39f5fce..856090f 100644 --- a/ifm3d_ros_msgs/package.xml +++ b/ifm3d_ros_msgs/package.xml @@ -2,11 +2,11 @@ ifm3d_ros_msgs - 0.1.0 + 0.2.0 ifm3d_ros messages subpackage - ifm CSR group + ifm robotics group Apache 2 - CSR ifm sytron + ifm robotics group https://github.com/ifm/ifm3d-ros/ @@ -16,7 +16,7 @@ std_msgs message_generation std_msgs - message_runtime + message_runtime tf2_ros diff --git a/ifm3d_ros_msgs/srv/DumpJSONSchema.srv b/ifm3d_ros_msgs/srv/DumpJSONSchema.srv new file mode 100644 index 0000000..1399d5b --- /dev/null +++ b/ifm3d_ros_msgs/srv/DumpJSONSchema.srv @@ -0,0 +1,3 @@ +--- +int32 status +string config