diff --git a/.devcontainer/scancontrol/devcontainer.json b/.devcontainer/scancontrol/devcontainer.json new file mode 100644 index 0000000..a514fa9 --- /dev/null +++ b/.devcontainer/scancontrol/devcontainer.json @@ -0,0 +1,23 @@ +// For format details, see https://aka.ms/devcontainer.json. For config options, see the README at: +// https://github.com/microsoft/vscode-dev-containers/tree/v0.217.4/containers/go +{ + "name": "scancontrol-devcontainer", + "dockerComposeFile": [ + "../../src/scancontrol/.docker/docker-compose.yml" + ], + "service": "scancontrol-service", + "remoteUser": "ros", + "workspaceFolder": "/home/ros/workspace/", + "customizations": { + "vscode": { + // Set *default* container specific settings.json values on container create. + "settings": { + }, + // Add the IDs of extensions you want installed when the container is created. + "extensions": [ + ] + } + } + // "postCreateCommand": "printenv | grep ROS" + // "forwardPorts": [], +} \ No newline at end of file diff --git a/.docker/Dockerfile b/.docker/Dockerfile deleted file mode 100644 index 0ead899..0000000 --- a/.docker/Dockerfile +++ /dev/null @@ -1,63 +0,0 @@ -FROM ubuntu:bionic AS build - -ARG DEBIAN_FRONTEND=noninteractive - -RUN apt-get update && \ - apt-get install -y --no-install-recommends \ - checkinstall \ - debhelper \ - g++ \ - gcc \ - git \ - intltool \ - libglib2.0-dev \ - libxml2-dev \ - meson \ - pkg-config \ - unzip \ - wget \ - && rm -rf /var/lib/apt/lists/* - -RUN wget http://ftp.acc.umu.se/pub/GNOME/sources/aravis/0.6/aravis-0.6.4.tar.xz && \ - tar xfJ aravis-0.6.4.tar.xz && \ - rm aravis-0.6.4.tar.xz && \ - cd aravis-0.6.4 && \ - ./configure && \ - make && \ - checkinstall --pkgname aravis --requires="libglib2.0-dev, libxml2-dev" && \ - ldconfig - -RUN wget https://software.micro-epsilon.com/scanCONTROL-Linux-SDK-0-2-4.zip && \ - unzip scanCONTROL-Linux-SDK-0-2-4.zip && \ - rm scanCONTROL-Linux-SDK-0-2-4.zip && \ - cd /scanCONTROL\ Linux\ SDK\ 0.2.4/libmescan/ && \ - meson builddir && \ - cd builddir && \ - ninja && \ - checkinstall --pkgname mescan --pkgversion 0.2.4 --requires="aravis \(\>= 0.6.0\)" ninja install && \ - ldconfig && \ - cd "/scanCONTROL Linux SDK 0.2.4/libllt/" && \ - meson builddir && \ - cd builddir && \ - ninja && \ - checkinstall --pkgname llt --pkgversion 0.2.4 --requires="mescan \(\>= 0.2.4\),aravis \(\>= 0.6.0\)" ninja install - -RUN mkdir /library_pkgs && \ - mv /aravis-0.6.4/aravis_0.6.4-1_amd64.deb /library_pkgs && \ - mv "/scanCONTROL Linux SDK 0.2.4/libmescan/builddir/mescan_0.2.4-1_amd64.deb" /library_pkgs && \ - mv "/scanCONTROL Linux SDK 0.2.4/libllt/builddir/llt_0.2.4-1_amd64.deb" /library_pkgs - -FROM ros:melodic-ros-core - -RUN apt-get update && \ - apt-get install -y \ - intltool \ - pkg-config \ - && rm -rf /var/lib/apt/lists/* - -COPY --from=build ["/library_pkgs", "/library_pkgs"] - -RUN apt-get update && \ - apt install -y /library_pkgs/aravis_0.6.4-1_amd64.deb && \ - apt install /library_pkgs/mescan_0.2.4-1_amd64.deb && \ - apt install /library_pkgs/llt_0.2.4-1_amd64.deb \ No newline at end of file diff --git a/.docker/Dockerfile.CI b/.docker/Dockerfile.CI new file mode 100644 index 0000000..2f1f11d --- /dev/null +++ b/.docker/Dockerfile.CI @@ -0,0 +1,85 @@ +ARG SCANCONTROL_SDK_VERSION=1.0.0 + +FROM ubuntu:22.04 AS build + +ARG SCANCONTROL_SDK_VERSION +ARG DEBIAN_FRONTEND=noninteractive + +RUN apt-get update && \ + apt-get install -y --no-install-recommends \ + checkinstall \ + cmake \ + debhelper \ + g++ \ + gcc \ + gettext \ + git \ + gobject-introspection \ + gtk-doc-tools \ + intltool \ + libgirepository1.0-dev \ + libglib2.0-dev \ + libgstreamer1.0-dev \ + libgstreamer-plugins-base1.0-dev \ + libgstreamer-plugins-good1.0-dev \ + libgtk-3-dev \ + libusb-1.0-0-dev \ + libxml2-dev \ + python3-pip \ + ninja-build \ + pkg-config \ + unzip \ + wget \ + xsltproc \ + meson \ + && rm -rf /var/lib/apt/lists/* + +RUN wget https://github.com/AravisProject/aravis/releases/download/0.8.30/aravis-0.8.30.tar.xz -O aravis-0.8.30.tar.xz &&\ + tar xfJ aravis-0.8.30.tar.xz && \ + rm aravis-0.8.30.tar.xz && \ + cd aravis-0.8.30 && \ + meson setup build && \ + cd build && \ + ninja && \ + checkinstall --pkgname aravis --pkgversion 0.8.30 --requires="libglib2.0-dev, libxml2-dev, libusb-1.0-0-dev" ninja install + +SHELL ["/bin/bash", "-c"] +RUN wget https://software.micro-epsilon.com/scanCONTROL-Linux-SDK-${SCANCONTROL_SDK_VERSION//./-}.zip -O scanCONTROL-Linux-SDK-${SCANCONTROL_SDK_VERSION//./-}.zip && \ + unzip scanCONTROL-Linux-SDK-${SCANCONTROL_SDK_VERSION//./-}.zip -d scanCONTROL-Linux-SDK/ && \ + rm scanCONTROL-Linux-SDK-${SCANCONTROL_SDK_VERSION//./-}.zip && \ + cd scanCONTROL-Linux-SDK/libmescan/ && \ + meson builddir && \ + cd builddir && \ + ninja && \ + checkinstall --pkgname mescan --pkgversion ${SCANCONTROL_SDK_VERSION} --requires="aravis \(\>= 0.8.0\)" ninja install && \ + ldconfig && \ + cd /scanCONTROL-Linux-SDK/libllt/include && \ + # scanCONTROL SDK fix for libglib + sed -i '27,29 s/^/\/\/ /; 31,33 s/^/\/\/ /;' llt.h && \ + cd /scanCONTROL-Linux-SDK/libllt && \ + meson builddir && \ + cd builddir && \ + ninja && \ + checkinstall --pkgname llt --pkgversion ${SCANCONTROL_SDK_VERSION} --requires="mescan \(\>= ${SCANCONTROL_SDK_VERSION}\),aravis \(\>= 0.8.0\)" ninja install + +RUN mkdir /library_pkgs && \ + mv /aravis-0.8.30/build/aravis_0.8.30-1_amd64.deb /library_pkgs && \ + mv "/scanCONTROL-Linux-SDK/libmescan/builddir/mescan_${SCANCONTROL_SDK_VERSION}-1_amd64.deb" /library_pkgs && \ + mv "/scanCONTROL-Linux-SDK/libllt/builddir/llt_${SCANCONTROL_SDK_VERSION}-1_amd64.deb" /library_pkgs + +FROM ros:humble-ros-core +ARG SCANCONTROL_SDK_VERSION + +RUN apt-get update && apt-get install -y --no-install-recommends\ + build-essential \ + pkg-config \ + && rm -rf /var/lib/apt/lists/* + +COPY --from=build ["/library_pkgs", "/library_pkgs"] + +RUN apt-get update && \ + apt install -y /library_pkgs/aravis_0.8.30-1_amd64.deb && \ + apt install /library_pkgs/mescan_${SCANCONTROL_SDK_VERSION}-1_amd64.deb && \ + apt install /library_pkgs/llt_${SCANCONTROL_SDK_VERSION}-1_amd64.deb \ + && rm -rf /var/lib/apt/lists/* + diff --git a/.docker/Dockerfile.dev b/.docker/Dockerfile.dev new file mode 100644 index 0000000..c5fb4f6 --- /dev/null +++ b/.docker/Dockerfile.dev @@ -0,0 +1,166 @@ +########################################### +# Base image +########################################### +FROM ubuntu:22.04 AS base + +ENV DEBIAN_FRONTEND=noninteractive + +# Install language +RUN apt-get update && apt-get install -y \ + locales \ + && locale-gen en_US.UTF-8 \ + && update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 \ + && rm -rf /var/lib/apt/lists/* +ENV LANG en_US.UTF-8 + +# Install timezone +RUN ln -fs /usr/share/zoneinfo/UTC /etc/localtime \ + && export DEBIAN_FRONTEND=noninteractive \ + && apt-get update \ + && apt-get install -y tzdata \ + && dpkg-reconfigure --frontend noninteractive tzdata \ + && rm -rf /var/lib/apt/lists/* + +RUN apt-get update && apt-get -y upgrade \ + && rm -rf /var/lib/apt/lists/* + +# Install common programs +RUN apt-get update && apt-get install -y --no-install-recommends \ + curl \ + gnupg2 \ + lsb-release \ + sudo \ + software-properties-common \ + wget \ + && rm -rf /var/lib/apt/lists/* + +# Install ROS2 base +RUN sudo add-apt-repository universe \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg \ + && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null \ + && apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-ros-base \ + python3-argcomplete \ + && rm -rf /var/lib/apt/lists/* + +ENV ROS_DISTRO=humble +ENV AMENT_PREFIX_PATH=/opt/ros/humble +ENV COLCON_PREFIX_PATH=/opt/ros/humble +ENV LD_LIBRARY_PATH=/opt/ros/humble/lib +ENV PATH=/opt/ros/humble/bin:$PATH +ENV PYTHONPATH=/opt/ros/humble/lib/python3.10/site-packages +ENV ROS_PYTHON_VERSION=3 +ENV ROS_VERSION=2 +ENV DEBIAN_FRONTEND= + +########################################### +# Develop image +########################################### +FROM base AS dev + +ENV DEBIAN_FRONTEND=noninteractive +RUN apt-get update && apt-get install -y --no-install-recommends \ + bash-completion \ + build-essential \ + cmake \ + gdb \ + git \ + openssh-client \ + python3-argcomplete \ + python3-pip \ + ros-dev-tools \ + ros-humble-ament-* \ + vim \ + && rm -rf /var/lib/apt/lists/* + +RUN rosdep init || echo "rosdep already initialized" + +ARG USERNAME=ros +ARG USER_UID=1000 +ARG USER_GID=$USER_UID + +# Create a non-root user +RUN groupadd --gid $USER_GID $USERNAME \ + && useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME \ + # Add sudo support for the non-root user + && apt-get update \ + && apt-get install -y sudo \ + && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME\ + && chmod 0440 /etc/sudoers.d/$USERNAME \ + && rm -rf /var/lib/apt/lists/* + +# Set up autocompletion for user +RUN apt-get update && apt-get install -y git-core bash-completion \ + && echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc \ + && echo "if [ -f /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash ]; then source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash; fi" >> /home/$USERNAME/.bashrc \ + && rm -rf /var/lib/apt/lists/* + +ENV DEBIAN_FRONTEND= +ENV AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 + +########################################### +# Full image +########################################### +FROM dev AS full + +ENV DEBIAN_FRONTEND=noninteractive +# Install the full release +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-desktop \ + && rm -rf /var/lib/apt/lists/* +ENV DEBIAN_FRONTEND= + +########################################### +# Workspace +########################################### +FROM full AS workspace + +ENV DEBIAN_FRONTEND=noninteractive +RUN apt-get update \ + && apt-get -y install --no-install-recommends \ + unzip \ + libxml2-dev cmake libusb-1.0-0-dev gobject-introspection \ + libgtk-3-dev gtk-doc-tools xsltproc libgstreamer1.0-dev \ + libgstreamer-plugins-base1.0-dev libgstreamer-plugins-good1.0-dev \ + libgirepository1.0-dev gettext \ + autotools-dev automake intltool libtool \ + meson \ + ninja-build \ + # Clean up + && apt-get autoremove -y \ + && apt-get clean -y \ + && rm -rf /var/lib/apt/lists/* + + +RUN wget https://github.com/AravisProject/aravis/releases/download/0.8.30/aravis-0.8.30.tar.xz && \ + tar xfJ aravis-0.8.30.tar.xz && \ + rm aravis-0.8.30.tar.xz + +RUN cd aravis-0.8.30 && \ + meson setup build && \ + cd build && \ + ninja && \ + ninja install && \ + ldconfig + +RUN wget https://software.micro-epsilon.com/scanCONTROL-Linux-SDK-1-0-0.zip -O scanCONTROL-Linux-SDK.zip && \ + unzip scanCONTROL-Linux-SDK.zip -d scanCONTROL-Linux-SDK/ && \ + rm scanCONTROL-Linux-SDK.zip + +RUN cd scanCONTROL-Linux-SDK/libmescan/ && \ + meson builddir && \ + cd builddir && \ + ninja install && \ + ldconfig + +RUN cd scanCONTROL-Linux-SDK/libllt/include && \ + wget -q https://raw.githubusercontent.com/Pugens/scancontrol/1105de0ea8a28526b03de488d76821e07bada265/micro_epsilon_scancontrol_driver/include/lltlib/llt.h -O llt.h && \ + cd .. && meson builddir && \ + cd builddir && \ + ninja && \ + ninja install && \ + ldconfig + +ENV DEBIAN_FRONTEND=dialog + +WORKDIR /home/ros/workspace \ No newline at end of file diff --git a/.docker/docker-compose.yml b/.docker/docker-compose.yml new file mode 100644 index 0000000..b28b515 --- /dev/null +++ b/.docker/docker-compose.yml @@ -0,0 +1,43 @@ +version: '3' + +name: scancontrol-compose +services: + scancontrol-service: + container_name: scancontrol-container + build: + context: . + dockerfile: Dockerfile.dev + user: ros + working_dir: /home/ros/workspace + # command: > + # sh -c "sudo apt-get update && + # rosdep update && + # rosdep install --from-paths . --ignore-src -y && + # source /opt/ros/humble/setup.bash && + # colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=RelWithDebInfo" "-DCMAKE_EXPORT_COMPILE_COMMANDS=On" -Wall -Wextra -Wpedantic && + # . install/setup.bash" + # Uncomment to expose graphic card drivers + # deploy: + # resources: + # reservations: + # devices: + # - driver: nvidia + # count: 1 + # capabilities: [gpu] + environment: + - DISPLAY=${DISPLAY} + - QT_X11_NO_MITSHM=1 + - XDG_RUNTIME_DIR=${XDG_RUNTIME_DIR} + # Uncomment to expose graphic card drivers + # - NVIDIA_DRIVER_CAPABILITIES=all + volumes: + - ../../../:/home/ros/workspace + - /var/run/docker.sock:/var/run/docker.sock + - /tmp/.X11-unix:/tmp/.X11-unix:rw + - ${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority + # Uncomment to use git within the devcontainer + # - ~/.ssh/:/home/ros/.ssh/ + network_mode: "host" #TODO: UNSAFE, change to specific port! + tty: true + init: true + \ No newline at end of file diff --git a/.github/workflows/industrial_ci_action.yaml b/.github/workflows/industrial_ci_action.yaml index 1b0bd94..875b329 100644 --- a/.github/workflows/industrial_ci_action.yaml +++ b/.github/workflows/industrial_ci_action.yaml @@ -7,15 +7,15 @@ jobs: strategy: matrix: env: - - {OS_NAME: ubuntu, OS_CODE_NAME: bionic, ROS_DISTRO: melodic, DOCKER_IMAGE: "samxl/scancontrol:latest"} + - {OS_NAME: ubuntu, OS_CODE_NAME: jammy, ROS_DISTRO: humble, DOCKER_IMAGE: "eugeniobernard/scancontrol2:sdk_v1.0.0"} env: CCACHE_DIR: /github/home/.ccache runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 - - uses: actions/cache@v2 + - uses: actions/checkout@v4 + - uses: actions/cache@v3 with: path: ${{ env.CCACHE_DIR }} key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} - uses: 'ros-industrial/industrial_ci@master' - env: ${{ matrix.env }} \ No newline at end of file + env: ${{ matrix.env }} diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/README.md b/README.md index 7caa940..ca1f1d8 100644 --- a/README.md +++ b/README.md @@ -1,86 +1,71 @@ # micro_epsilon_scancontrol -![CI](https://github.com/sam-xl/scancontrol/workflows/CI/badge.svg) [![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) +![CI](https://github.com/sam-xl/scancontrol/workflows/CI/badge.svg) +[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) ## Overview +ROS device driver for the scanCONTROL series of laser line scanners of Micro Epsilon using the [scanCONTROL Linux C++ SDK 1.0]. The driver allows to connect to a (specific) scanCONTROL device, configure the sensor using predefined settings or at runtime and publishes the sensor data as point clouds. -ROS device driver for the scanCONTROL series of laser line scanners of Micro Epsilon using the [scanCONTROL Linux C++ SDK 0.2]. The driver allows to connect to a (specific) scanCONTROL device, configure the sensor using predefined settings or at runtime and publishes the sensor data as point clouds. +Author: **D. Kroezen** \ +Affiliation: [SAM XL](https://samxl.com/), [TU Delft](https://tudelft.nl/) \ +Maintainers: +**D. Kroezen, d.kroezen@tudelft.nl**, **E. Bernardi, e.bernardi@tudelft.nl** -**Author: D. Kroezen (GitHub username: dave992)
-Affiliation: [SAM|XL](https://samxl.com/), [TU Delft](https://tudelft.nl/)
-Maintainer: D. Kroezen, d.kroezen@tudelft.nl** +> [!WARNING] +> This micro_epsilon_scancontrol package branch **is under development** for [ROS2 Humble](https://docs.ros.org/en/humble/index.html) and Ubuntu 22.04 Jammy! -The micro_epsilon_scancontrol package has been tested under [ROS] Melodic and Ubuntu 18.04. ## Installation #### Dependencies - -- [Aravis 0.6.x](https://github.com/AravisProject/aravis) -- [scanCONTROL Linux C++ SDK 0.2.4](https://www.micro-epsilon.com/2D_3D/laser-scanner/Software/downloads/) - - - - +- [Aravis 0.8.x](https://github.com/AravisProject/aravis/releases) [[docs]](https://aravisproject.github.io/aravis/) +- [scanCONTROL Linux C++ SDK 1.0.x](https://www.micro-epsilon.com/2D_3D/laser-scanner/Software/downloads/) #### Building - -To build from source, clone the latest version from this repository into your catkin workspace and compile the package using: - - cd catkin_ws/src - git clone https://github.com/sam-xl/micro_epsilon_scancontrol.git - cd ../ - catkin build - - - - -## Usage - -Run the main driver node with: - - roslaunch micro_epsilon_scancontrol_driver load_driver.launch - - - -## Launch files - -* **driver.launch:** Launch a single scanCONTROL driver node and the configuration window. - +To build from source, clone the latest version from this repository into your workspace and compile the package: + +```bash +mkdir -p ros2_ws/src && cd ros2_ws +git clone git@github.com:sam-xl/scancontrol.git -b ros2-devel src +``` +To build locally, after installing Aravis and the SDK: +```bash +sudo apt-get update +rosdep update +rosdep install --from-paths src --ignore-src -y +colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=RelWithDebInfo" "-DCMAKE_EXPORT_COMPILE_COMMANDS=On" -Wall -Wextra -Wpedantic +``` +And source the installation +```bash +. install/setup.bash +``` +## Development setup +For anyone having access to scanCONTROL hardware and not wanting to install dependencies locally, follow the setup below to work with docker compose or in a devcontainer environment. + +#### Docker compose: +- `cd src/scancontrol/.docker` +- `docker compose up -d --build` +- If using VSCode, you can attach a shell or VSCode and work from there. +- Or you can use other dev IDEs? **TODO**: + - Add [watch](https://docs.docker.com/compose/file-watch/) sections to one or more services in compose.yaml + - Run docker compose watch to build and launch a Compose project and start the file watch mode. + - Edit service source files using your preferred IDE or editor +#### devcontainer: +- Link the `.devcontainer` folder + - cd to your `ros2_ws` + - `cp src/scancontrol/.devcontainer/ .` + - Hit `F1` in VSCode and select `build and reaopen in container` ## Nodes +### scancontrol_description_node +Visualise the scanCONTROL models +```bash +ros2 launch micro_epsilon_scancontrol_description load_scancontrol.launch.py +``` + +Possible models passed as arguments with `scancontrol_type:=` +- `scancontrol_26x0_29x0_25.xacro` (default) +- `scancontrol_27x0_100.xacro` +- `scancontrol_30xx_25.xacro` ### scancontrol_driver_node @@ -94,7 +79,8 @@ The scancontrol_driver_node connects to the scanCONTROL device and allows contro #### Services -Most servives are wrappers of the scanCONTROL API. For more information on the available settings and values see the documentation as part of the [scanCONTROL Linux C++ SDK 0.2](https://www.micro-epsilon.com/2D_3D/laser-scanner/Software/downloads/). The rqt plugin uses these services to change the settings during runtime. +Most servives are wrappers of the scanCONTROL API. +The rqt plugin uses these services to change the settings during runtime. * **`~set_feature`** ([micro_epsilon_scancontrol_msgs/SetFeature]) diff --git a/micro_epsilon_scancontrol_description/CMakeLists.txt b/micro_epsilon_scancontrol_description/CMakeLists.txt index 42aeeef..1cb3f93 100644 --- a/micro_epsilon_scancontrol_description/CMakeLists.txt +++ b/micro_epsilon_scancontrol_description/CMakeLists.txt @@ -1,10 +1,10 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(micro_epsilon_scancontrol_description) -find_package(catkin REQUIRED) +find_package(ament_cmake REQUIRED) -catkin_package() +ament_package() install(DIRECTORY config launch meshes urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + DESTINATION share/${PROJECT_NAME}) diff --git a/micro_epsilon_scancontrol_description/config/rviz.rviz b/micro_epsilon_scancontrol_description/config/rviz.rviz index a604c76..86bad99 100644 --- a/micro_epsilon_scancontrol_description/config/rviz.rviz +++ b/micro_epsilon_scancontrol_description/config/rviz.rviz @@ -1,44 +1,34 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /TF1 - - /TF1/Frames1 + Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 815 - - Class: rviz/Selection + Tree Height: 793 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -51,8 +41,16 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: All Links Enabled: true @@ -75,52 +73,18 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.01 - Style: Points - Topic: /profiles - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: false - aperture_frame: - Value: false - base_link: - Value: true - Marker Scale: 0.5 + Marker Scale: 0.30000001192092896 Name: TF Show Arrows: false Show Axes: true - Show Names: true + Show Names: false Tree: - base_link: + world: aperture_frame: {} Update Interval: 0 @@ -128,59 +92,82 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: base_link + Fixed Frame: world Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 1.29337 + Class: rviz_default_plugins/Orbit + Distance: 0.5106813907623291 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0 - Y: 0 - Z: 0 + X: -0.10232929140329361 + Y: 0.004503462463617325 + Z: 0.20709049701690674 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.785398 - Target Frame: base_link + Near Clip Distance: 0.009999999776482582 + Pitch: 0.41979584097862244 + Target Frame: Value: Orbit (rviz) - Yaw: 0.785398 + Yaw: 1.9503984451293945 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1028 + Height: 1016 Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a000003befc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003be000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003befc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000003be000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b0000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000013b000003be00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000224000003a2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000556000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false - Time: - collapsed: false Tool Properties: collapsed: false Views: - collapsed: false - Width: 960 - X: -10 - Y: 14 + collapsed: true + Width: 1920 + X: 0 + Y: 27 diff --git a/micro_epsilon_scancontrol_description/launch/load_scancontrol.launch.py b/micro_epsilon_scancontrol_description/launch/load_scancontrol.launch.py new file mode 100644 index 0000000..222968f --- /dev/null +++ b/micro_epsilon_scancontrol_description/launch/load_scancontrol.launch.py @@ -0,0 +1,102 @@ +# Copyright 2023 SAM XL (Eugenio Bernardi) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + LaunchConfiguration, + Command, + PathJoinSubstitution, + FindExecutable, +) +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + declared_arguments = [] + # General arguments + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="micro_epsilon_scancontrol_description", + description="Description package with URDF/XACRO files. Usually the argument \ + is not set, it enables use of a custom description.", + ) + ) + # Declare argument for the URDF file selection + declared_arguments.append( + DeclareLaunchArgument( + "scancontrol_type", + default_value="scancontrol_26x0_29x0_25.xacro", + description="scanCONTROL URDF type file to visualize", + choices=[ + "scancontrol_26x0_29x0_25.xacro", + "scancontrol_27x0_100.xacro", + "scancontrol_30xx_25.xacro", + ], + ) + ) + + # General arguments + description_package = LaunchConfiguration("description_package") + # Initialize Arguments + scancontrol_type = LaunchConfiguration("scancontrol_type") + + # Get the path to the URDF file + urdf_file_path = os.path.join( + get_package_share_directory("micro_epsilon_scancontrol_description"), "urdf" + ) + robot_description = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", scancontrol_type] + ) + ] + ) + + # Node to publish the URDF to the robot_description parameter + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + # output='both', + output="screen", + parameters=[{"robot_description": robot_description}], + ) + + # Node to start RViz2 + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + get_package_share_directory("micro_epsilon_scancontrol_description") + + "/config/rviz.rviz", + ], + ) + + return LaunchDescription( + declared_arguments + + [ + robot_state_publisher_node, + rviz_node, + ] + ) diff --git a/micro_epsilon_scancontrol_description/launch/load_scancontrol_26x0_29x0_25.launch b/micro_epsilon_scancontrol_description/launch/load_scancontrol_26x0_29x0_25.launch deleted file mode 100644 index e9af850..0000000 --- a/micro_epsilon_scancontrol_description/launch/load_scancontrol_26x0_29x0_25.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/micro_epsilon_scancontrol_description/launch/load_scancontrol_27x0_100.launch b/micro_epsilon_scancontrol_description/launch/load_scancontrol_27x0_100.launch deleted file mode 100644 index 3959768..0000000 --- a/micro_epsilon_scancontrol_description/launch/load_scancontrol_27x0_100.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/micro_epsilon_scancontrol_description/launch/load_scancontrol_30xx_25.launch b/micro_epsilon_scancontrol_description/launch/load_scancontrol_30xx_25.launch deleted file mode 100644 index b52b016..0000000 --- a/micro_epsilon_scancontrol_description/launch/load_scancontrol_30xx_25.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/micro_epsilon_scancontrol_description/launch/test_scancontrol_26x0_29x0_25.launch b/micro_epsilon_scancontrol_description/launch/test_scancontrol_26x0_29x0_25.launch deleted file mode 100644 index d3ba2bc..0000000 --- a/micro_epsilon_scancontrol_description/launch/test_scancontrol_26x0_29x0_25.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/micro_epsilon_scancontrol_description/launch/test_scancontrol_27x0_100.launch b/micro_epsilon_scancontrol_description/launch/test_scancontrol_27x0_100.launch deleted file mode 100644 index 03bc79d..0000000 --- a/micro_epsilon_scancontrol_description/launch/test_scancontrol_27x0_100.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/micro_epsilon_scancontrol_description/launch/test_scancontrol_30xx_25.launch b/micro_epsilon_scancontrol_description/launch/test_scancontrol_30xx_25.launch deleted file mode 100644 index a454e1e..0000000 --- a/micro_epsilon_scancontrol_description/launch/test_scancontrol_30xx_25.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/micro_epsilon_scancontrol_description/meshes/collision/collision.stl b/micro_epsilon_scancontrol_description/meshes/collision/26x0_29x0_25.stl similarity index 100% rename from micro_epsilon_scancontrol_description/meshes/collision/collision.stl rename to micro_epsilon_scancontrol_description/meshes/collision/26x0_29x0_25.stl diff --git a/micro_epsilon_scancontrol_description/meshes/visual/visual.dae b/micro_epsilon_scancontrol_description/meshes/visual/26x0_29x0_25.dae similarity index 100% rename from micro_epsilon_scancontrol_description/meshes/visual/visual.dae rename to micro_epsilon_scancontrol_description/meshes/visual/26x0_29x0_25.dae diff --git a/micro_epsilon_scancontrol_description/package.xml b/micro_epsilon_scancontrol_description/package.xml index 4ed225f..c9d5ae4 100644 --- a/micro_epsilon_scancontrol_description/package.xml +++ b/micro_epsilon_scancontrol_description/package.xml @@ -1,20 +1,24 @@ - + micro_epsilon_scancontrol_description 0.1.0 URDFs for the Micro-Epsilon scanCONTROL laser profile sensors. - Rik Tonnaer (Delft University of Technology) - BSD + Eugenio Bernardi (Delft University of Technology) + + https://github.com/sam-xl/scancontrol + + Apache 2.0 - catkin + ament_cmake - joint_state_publisher - robot_state_publisher - rviz - xacro + joint_state_publisher + robot_state_publisher + rviz2 + xacro + ament_cmake diff --git a/micro_epsilon_scancontrol_description/urdf/scancontrol_26x0_29x0_25_macro.xacro b/micro_epsilon_scancontrol_description/urdf/scancontrol_26x0_29x0_25_macro.xacro index 1986fe0..cdcdcd5 100644 --- a/micro_epsilon_scancontrol_description/urdf/scancontrol_26x0_29x0_25_macro.xacro +++ b/micro_epsilon_scancontrol_description/urdf/scancontrol_26x0_29x0_25_macro.xacro @@ -7,7 +7,7 @@ - + @@ -16,7 +16,7 @@ - + diff --git a/micro_epsilon_scancontrol_driver/CMakeLists.txt b/micro_epsilon_scancontrol_driver/CMakeLists.txt index 17c6344..5b29478 100644 --- a/micro_epsilon_scancontrol_driver/CMakeLists.txt +++ b/micro_epsilon_scancontrol_driver/CMakeLists.txt @@ -1,21 +1,46 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(micro_epsilon_scancontrol_driver) -find_package(catkin REQUIRED COMPONENTS roscpp pcl_ros nodelet micro_epsilon_scancontrol_msgs) -find_package(PkgConfig REQUIRED) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -pkg_check_modules(scancontrol REQUIRED glib-2.0 aravis-0.6 mescan llt) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_srvs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(PkgConfig REQUIRED) +find_package(micro_epsilon_scancontrol_msgs REQUIRED) -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp pcl_ros nodelet micro_epsilon_scancontrol_msgs - DEPENDS scancontrol -) +pkg_check_modules(SCANCONTROL REQUIRED glib-2.0 aravis-0.8 mescan llt) -include_directories(include ${catkin_INCLUDE_DIRS} ${scancontrol_INCLUDE_DIRS}) +include_directories(include ${SCANCONTROL_INCLUDE_DIRS}) add_executable(driver_node src/node.cpp src/driver.cpp) -target_link_libraries(driver_node ${catkin_LIBRARIES} ${scancontrol_LIBRARIES}) +ament_target_dependencies(driver_node + rclcpp + std_srvs + sensor_msgs + pcl_ros + pcl_conversions + rclcpp_components + micro_epsilon_scancontrol_msgs +) +target_link_libraries(driver_node + "${SCANCONTROL_LIBRARIES}") + +install(TARGETS driver_node + DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY include/ + DESTINATION include) +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) +ament_export_include_directories(include) +ament_export_libraries(driver_node ) #driver_nodelet -add_library(driver_nodelet src/nodelet.cpp src/driver.cpp) -target_link_libraries(driver_nodelet ${catkin_LIBRARIES} ${scancontrol_LIBRARIES}) +ament_package() diff --git a/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h b/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h index 1c20975..4c722de 100644 --- a/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h +++ b/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h @@ -1,16 +1,21 @@ #ifndef _SCANCONTROL_DRIVER_H_ #define _SCANCONTROL_DRIVER_H_ -#include +// C++ +#include + +// ROS +#include #include -#include +#include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -30,7 +35,7 @@ namespace scancontrol_driver { public: // Constructor and destructor - ScanControlDriver(ros::NodeHandle nh, ros::NodeHandle private_nh); + ScanControlDriver(rclcpp::Node::SharedPtr& nh, rclcpp::Node::SharedPtr& private_nh); ~ScanControlDriver() {} // Profile functions @@ -48,13 +53,27 @@ namespace scancontrol_driver int resolution() const {return config_.resolution;}; // Service Callback - bool ServiceSetFeature(micro_epsilon_scancontrol_msgs::SetFeature::Request &request, micro_epsilon_scancontrol_msgs::SetFeature::Response &response); - bool ServiceGetFeature(micro_epsilon_scancontrol_msgs::GetFeature::Request &request, micro_epsilon_scancontrol_msgs::GetFeature::Response &response); - bool ServiceSetResolution(micro_epsilon_scancontrol_msgs::SetResolution::Request &request, micro_epsilon_scancontrol_msgs::SetResolution::Response &response); - bool ServiceGetResolution(micro_epsilon_scancontrol_msgs::GetResolution::Request &request, micro_epsilon_scancontrol_msgs::GetResolution::Response &response); - bool ServiceGetAvailableResolutions(micro_epsilon_scancontrol_msgs::GetAvailableResolutions::Request &request, micro_epsilon_scancontrol_msgs::GetAvailableResolutions::Response &response); - bool ServiceInvertZ(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response); - bool ServiceInvertX(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response); + void ServiceSetFeature( + const std::shared_ptr request, + std::shared_ptr response); + void ServiceGetFeature( + const std::shared_ptr request, + std::shared_ptr response); + void ServiceSetResolution( + const std::shared_ptr request, + std::shared_ptr response); + void ServiceGetResolution( + const std::shared_ptr request, + std::shared_ptr response); + void ServiceGetAvailableResolutions( + const std::shared_ptr request, + std::shared_ptr response); + void ServiceInvertZ( + const std::shared_ptr request, + std::shared_ptr response); + void ServiceInvertX( + const std::shared_ptr request, + std::shared_ptr response); private: // Profile functions @@ -78,16 +97,17 @@ namespace scancontrol_driver bool transfer_active_ = false; // ROS handles - ros::NodeHandle nh_; - ros::NodeHandle private_nh_; - ros::Publisher publisher; - ros::ServiceServer get_feature_srv; - ros::ServiceServer set_feature_srv; - ros::ServiceServer get_resolution_srv; - ros::ServiceServer set_resolution_srv; - ros::ServiceServer get_available_resolutions_srv; - ros::ServiceServer invert_z_srv; - ros::ServiceServer invert_x_srv; + rclcpp::Node::SharedPtr nh_; + rclcpp::Node::SharedPtr private_nh_; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Service::SharedPtr get_feature_srv; + rclcpp::Service::SharedPtr set_feature_srv; + rclcpp::Service::SharedPtr get_resolution_srv; + rclcpp::Service::SharedPtr set_resolution_srv; + rclcpp::Service::SharedPtr get_available_resolutions_srv; + rclcpp::Service::SharedPtr invert_z_srv; + rclcpp::Service::SharedPtr invert_x_srv; + // Driver objects diff --git a/micro_epsilon_scancontrol_driver/launch/load_driver.launch b/micro_epsilon_scancontrol_driver/launch/load_driver.launch deleted file mode 100644 index 1eef064..0000000 --- a/micro_epsilon_scancontrol_driver/launch/load_driver.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/micro_epsilon_scancontrol_driver/launch/load_driver.launch.py b/micro_epsilon_scancontrol_driver/launch/load_driver.launch.py new file mode 100644 index 0000000..e26daf6 --- /dev/null +++ b/micro_epsilon_scancontrol_driver/launch/load_driver.launch.py @@ -0,0 +1,22 @@ +import os +import sys + +import launch +import launch_ros.actions + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch_ros.actions.Node( + package='micro_epsilon_scancontrol_driver', + executable='driver_node', + name='scancontrol_driver', + output='screen', + on_exit=launch.actions.Shutdown() + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/micro_epsilon_scancontrol_driver/launch/test_driver.launch b/micro_epsilon_scancontrol_driver/launch/test_driver.launch deleted file mode 100644 index 98ad22a..0000000 --- a/micro_epsilon_scancontrol_driver/launch/test_driver.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/micro_epsilon_scancontrol_driver/launch/test_driver.launch.py b/micro_epsilon_scancontrol_driver/launch/test_driver.launch.py new file mode 100644 index 0000000..0b5a944 --- /dev/null +++ b/micro_epsilon_scancontrol_driver/launch/test_driver.launch.py @@ -0,0 +1,50 @@ +import os +import sys + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = launch.LaunchDescription([ + launch_ros.actions.Node( + package='micro_epsilon_scancontrol_driver', + executable='driver_node', + name='scancontrol_driver', + output='screen', + on_exit=launch.actions.Shutdown() + ), + launch_ros.actions.Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[ + { + 'use_gui': 'false' + } + ] + ), + launch_ros.actions.Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher' + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz2', + on_exit=launch.actions.Shutdown() + ), + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory( + 'micro_epsilon_scancontrol_description'), 'launch/load_scancontrol.launch.py') + ) + ) + ]) + return ld + + +if __name__ == '__main__': + generate_launch_description() diff --git a/micro_epsilon_scancontrol_driver/me_scancontrol_nodelet.xml b/micro_epsilon_scancontrol_driver/me_scancontrol_nodelet.xml deleted file mode 100644 index 5bbcc95..0000000 --- a/micro_epsilon_scancontrol_driver/me_scancontrol_nodelet.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - Publish scanCONTROL pointclouds - - - \ No newline at end of file diff --git a/micro_epsilon_scancontrol_driver/package.xml b/micro_epsilon_scancontrol_driver/package.xml index 2012132..4a603ff 100644 --- a/micro_epsilon_scancontrol_driver/package.xml +++ b/micro_epsilon_scancontrol_driver/package.xml @@ -1,24 +1,26 @@ - + micro_epsilon_scancontrol_driver - 0.0.1 - Micro Espsilon scanCONTROL sensor driver - Rik Tonnear + 0.1.0 + Micro Espsilon scanCONTROL driver + Dave Kroezen https://github.com/sam-xl/scancontrol Apache 2.0 - catkin + ament_cmake - roscpp + rclcpp + std_srvs + pcl_conversions pcl_ros - nodelet + sensor_msgs micro_epsilon_scancontrol_msgs - + ament_cmake diff --git a/micro_epsilon_scancontrol_driver/src/driver.cpp b/micro_epsilon_scancontrol_driver/src/driver.cpp index af2dcac..e2360a7 100644 --- a/micro_epsilon_scancontrol_driver/src/driver.cpp +++ b/micro_epsilon_scancontrol_driver/src/driver.cpp @@ -2,7 +2,10 @@ namespace scancontrol_driver { - ScanControlDriver::ScanControlDriver(ros::NodeHandle nh, ros::NodeHandle private_nh) + + static const rclcpp::Logger LOGGER = rclcpp::get_logger("scancontrol_driver"); + + ScanControlDriver::ScanControlDriver(rclcpp::Node::SharedPtr& nh, rclcpp::Node::SharedPtr& private_nh) { /* Store the ros::NodeHandle objects and extract the relevant parameters. @@ -11,18 +14,26 @@ namespace scancontrol_driver private_nh_ = private_nh; // Device settings - private_nh_.param("resolution", config_.resolution, -1); + private_nh_->declare_parameter("resolution", -1); + private_nh_->get_parameter_or("resolution", config_.resolution, -1); - // Multiple device parameters - private_nh_.param("serial", config_.serial, std::string("")); - private_nh_.param("frame_id", config_.frame_id, std::string(DEFAULT_FRAME_ID)); - private_nh_.param("topic_name", config_.topic_name, std::string(DEFAULT_TOPIC_NAME)); + // Multiple device parameters + private_nh_->declare_parameter("serial", std::string("")); + private_nh_->get_parameter_or("serial", config_.serial, std::string("")); + private_nh_->declare_parameter("frame_id", std::string(DEFAULT_FRAME_ID)); + private_nh_->get_parameter_or("frame_id", config_.frame_id, std::string(DEFAULT_FRAME_ID)); + private_nh_->declare_parameter("topic_name", std::string(DEFAULT_TOPIC_NAME)); + private_nh_->get_parameter_or("topic_name", config_.topic_name, std::string(DEFAULT_TOPIC_NAME)); // TODO: Are these parameters needed? - private_nh_.param("partial_profile_start_point", config_.pp_start_point, 0); - private_nh_.param("partial_profile_start_point_data", config_.pp_start_point_data, 4); - private_nh_.param("partial_profile_point_count", config_.pp_point_count, -1); - private_nh_.param("partial_profile_data_width", config_.pp_point_data_width, 4); + private_nh_->declare_parameter("partial_profile_start_point", 0); + private_nh_->get_parameter_or("partial_profile_start_point", config_.pp_start_point, 0); + private_nh_->declare_parameter("partial_profile_start_point_data", 4); + private_nh_->get_parameter_or("partial_profile_start_point_data", config_.pp_start_point_data, 4); + private_nh_->declare_parameter("partial_profile_point_count", -1); + private_nh_->get_parameter_or("partial_profile_point_count", config_.pp_point_count, -1); + private_nh_->declare_parameter("partial_profile_data_width", 4); + private_nh_->get_parameter_or("partial_profile_data_width", config_.pp_point_data_width, 4); // Create driver interface object: device_interface_ptr = std::make_unique(); @@ -41,10 +52,10 @@ namespace scancontrol_driver return_code = device_interface_ptr->GetDeviceInterfaces(&available_interfaces[0], MAX_DEVICE_INTERFACE_COUNT); if (return_code == ERROR_GETDEVINTERFACE_REQUEST_COUNT){ - ROS_WARN_STREAM("There are more than " << MAX_DEVICE_INTERFACE_COUNT << " scanCONTROL sensors connected."); + RCLCPP_WARN_STREAM(LOGGER, "There are more than " << MAX_DEVICE_INTERFACE_COUNT << " scanCONTROL sensors connected."); interface_count = MAX_DEVICE_INTERFACE_COUNT; } else if (return_code < 0) { - ROS_WARN_STREAM("An error occured while searching for connected scanCONTROL devices. Code: " << return_code); + RCLCPP_WARN_STREAM(LOGGER, "An error occured while searching for connected scanCONTROL devices. Code: " << return_code); interface_count = 0; } else { interface_count = return_code; @@ -56,47 +67,47 @@ namespace scancontrol_driver */ gint8 selected_interface = -1; if (interface_count == 0){ - ROS_WARN("There is no scanCONTROL device connected. Exiting..."); + RCLCPP_WARN(LOGGER, "There is no scanCONTROL device connected. Exiting..."); goto stop_initialization; } else if (interface_count == 1){ - ROS_INFO("There is 1 scanCONTROL device connected."); + RCLCPP_INFO(LOGGER, "There is 1 scanCONTROL device connected."); selected_interface = 0; // Check if the available device is the same as the prefered device (if a serial is provided): std::string interface(available_interfaces[0]); if ((config_.serial == "") || (interface.find(config_.serial) > -1)){ - ROS_INFO_STREAM("Interface found: " << interface); + RCLCPP_INFO_STREAM(LOGGER, "Interface found: " << interface); } else{ - ROS_WARN_STREAM("Interface not found! Searched for serial = " << config_.serial); - ROS_INFO_STREAM("Selected interface: " << interface); + RCLCPP_WARN_STREAM(LOGGER, "Interface not found! Searched for serial = " << config_.serial); + RCLCPP_INFO_STREAM(LOGGER, "Selected interface: " << interface); } } else { - ROS_INFO_STREAM("There are " << interface_count << " scanCONTROL devices connected."); + RCLCPP_INFO_STREAM(LOGGER, "There are " << interface_count << " scanCONTROL devices connected."); // Select prefered device based on the defined ip or serial. If both are set, this selects the device which ip or serial is encountered first. if (config_.serial != ""){ for (int i = 0; i < interface_count; i++){ std::string interface(available_interfaces[i]); if (interface.find(config_.serial) > -1){ - ROS_INFO_STREAM("Interface found: " << interface); + RCLCPP_INFO_STREAM(LOGGER, "Interface found: " << interface); selected_interface = i; break; } } // Fallback if serial are not found: if (selected_interface == -1){ - ROS_WARN_STREAM("Interface not found! Searched for serial = " << config_.serial); - ROS_WARN("Available interfaces:"); + RCLCPP_WARN_STREAM(LOGGER, "Interface not found! Searched for serial = " << config_.serial); + RCLCPP_WARN(LOGGER, "Available interfaces:"); for (gint8 i = 0; i < interface_count; i++){ - ROS_WARN_STREAM(" " << available_interfaces[i]); + RCLCPP_WARN_STREAM(LOGGER, " " << available_interfaces[i]); } selected_interface = 0; - ROS_INFO_STREAM("\nSelecting first available interface: " << available_interfaces[selected_interface]); + RCLCPP_INFO_STREAM(LOGGER, "\nSelecting first available interface: " << available_interfaces[selected_interface]); } } else{ selected_interface = 0; - ROS_INFO_STREAM("No 'serial' set, selecting first interface: " << available_interfaces[selected_interface]); + RCLCPP_INFO_STREAM(LOGGER, "No 'serial' set, selecting first interface: " << available_interfaces[selected_interface]); } } @@ -107,7 +118,7 @@ namespace scancontrol_driver config_.serial = std::string(config_.interface.end() - 9, config_.interface.end()); return_code = device_interface_ptr->SetDeviceInterface(available_interfaces[selected_interface]); if (return_code < GENERAL_FUNCTION_OK){ - ROS_FATAL_STREAM("Error while setting device ID! Code: " << return_code); + RCLCPP_FATAL_STREAM(LOGGER, "Error while setting device ID! Code: " << return_code); goto stop_initialization; } @@ -116,7 +127,7 @@ namespace scancontrol_driver */ return_code = device_interface_ptr->Connect(); if (return_code < GENERAL_FUNCTION_OK){ - ROS_FATAL_STREAM("Error while connecting to device! Code: " << return_code); + RCLCPP_FATAL_STREAM(LOGGER, "Error while connecting to device! Code: " << return_code); goto stop_initialization; } @@ -125,26 +136,26 @@ namespace scancontrol_driver */ return_code = device_interface_ptr->GetLLTType(&device_type); if (return_code < GENERAL_FUNCTION_OK){ - ROS_FATAL_STREAM("Error while retrieving device type! Code: " << return_code); + RCLCPP_FATAL_STREAM(LOGGER, "Error while retrieving device type! Code: " << return_code); goto stop_initialization; } if (device_type >= scanCONTROL27xx_25 && device_type <= scanCONTROL27xx_xxx) { - ROS_INFO_STREAM("The scanCONTROL is a scanCONTROL27xx, with serial number " << config_.serial << "."); + RCLCPP_INFO_STREAM(LOGGER, "The scanCONTROL is a scanCONTROL27xx, with serial number " << config_.serial << "."); config_.model = std::string("scanCONTROL27xx"); } else if (device_type >= scanCONTROL26xx_25 && device_type <= scanCONTROL26xx_xxx) { - ROS_INFO_STREAM("The scanCONTROL is a scanCONTROL26xx, with serial number " << config_.serial << "."); + RCLCPP_INFO_STREAM(LOGGER, "The scanCONTROL is a scanCONTROL26xx, with serial number " << config_.serial << "."); config_.model = std::string("scanCONTROL26xx"); } else if (device_type >= scanCONTROL29xx_25 && device_type <= scanCONTROL29xx_xxx) { - ROS_INFO_STREAM("The scanCONTROL is a scanCONTROL29xx, with serial number " << config_.serial << "."); + RCLCPP_INFO_STREAM(LOGGER, "The scanCONTROL is a scanCONTROL29xx, with serial number " << config_.serial << "."); config_.model = std::string("scanCONTROL29xx"); } else if (device_type >= scanCONTROL30xx_25 && device_type <= scanCONTROL30xx_xxx) { - ROS_INFO_STREAM("The scanCONTROL is a scanCONTROL30xx, with serial number " << config_.serial << "."); + RCLCPP_INFO_STREAM(LOGGER, "The scanCONTROL is a scanCONTROL30xx, with serial number " << config_.serial << "."); config_.model = std::string("scanCONTROL30xx"); } else if (device_type >= scanCONTROL25xx_25 && device_type <= scanCONTROL25xx_xxx) { - ROS_INFO_STREAM("The scanCONTROL is a scanCONTROL25xx, with serial number " << config_.serial << "."); + RCLCPP_INFO_STREAM(LOGGER, "The scanCONTROL is a scanCONTROL25xx, with serial number " << config_.serial << "."); config_.model = std::string("scanCONTROL25xx"); } else { - ROS_FATAL("The scanCONTROL device is a undefined type.\nPlease contact Micro-Epsilon for a newer SDK or update the driver."); + RCLCPP_FATAL(LOGGER, "The scanCONTROL device is a undefined type.\nPlease contact Micro-Epsilon for a newer SDK or update the driver."); goto stop_initialization; } @@ -154,7 +165,7 @@ namespace scancontrol_driver // Find all available resolutions guint32 available_resolutions[MAX_RESOLUTION_COUNT]; if (return_code = device_interface_ptr->GetResolutions(&available_resolutions[0], MAX_RESOLUTION_COUNT) < GENERAL_FUNCTION_OK){ - ROS_FATAL_STREAM("Unable to request the available resolutions of the scanCONTROL device. Code: " << return_code); + RCLCPP_FATAL_STREAM(LOGGER, "Unable to request the available resolutions of the scanCONTROL device. Code: " << return_code); goto stop_initialization; } @@ -169,22 +180,22 @@ namespace scancontrol_driver } } if (selected_resolution == -1){ - ROS_WARN_STREAM("Requested resolution of " << std::to_string(config_.resolution) <<" not found as available option."); - ROS_WARN("Available resolutions:"); + RCLCPP_WARN_STREAM(LOGGER, "Requested resolution of " << std::to_string(config_.resolution) <<" not found as available option."); + RCLCPP_WARN(LOGGER, "Available resolutions:"); for (int i = 0; i < return_code; i++){ - ROS_WARN_STREAM(" " << std::to_string(available_resolutions[i])); + RCLCPP_WARN_STREAM(LOGGER, " " << std::to_string(available_resolutions[i])); } config_.resolution = available_resolutions[0]; - ROS_INFO_STREAM("Selecting first available resolution: " << std::to_string(config_.resolution)); + RCLCPP_INFO_STREAM(LOGGER, "Selecting first available resolution: " << std::to_string(config_.resolution)); } } else{ config_.resolution = available_resolutions[0]; - ROS_INFO_STREAM("No resolution set, selecting first available: " << std::to_string(config_.resolution)); + RCLCPP_INFO_STREAM(LOGGER, "No resolution set, selecting first available: " << std::to_string(config_.resolution)); } // Set the selected resolution if (return_code = device_interface_ptr->SetResolution(config_.resolution) < GENERAL_FUNCTION_OK){ - ROS_FATAL_STREAM("Error while setting device resolution! CodeL " << return_code); + RCLCPP_FATAL_STREAM(LOGGER, "Error while setting device resolution! CodeL " << return_code); goto stop_initialization; } @@ -201,11 +212,11 @@ namespace scancontrol_driver RegisterControlLostCallback > ControlLostCallback: Triggered when control of the device is lost. */ if ((return_code = device_interface_ptr->RegisterBufferCallback((gpointer) &NewProfileCallback, this)) < GENERAL_FUNCTION_OK){ - ROS_FATAL_STREAM("Error while registering buffer callback. Code: " << return_code); + RCLCPP_FATAL_STREAM(LOGGER, "Error while registering buffer callback. Code: " << return_code); goto stop_initialization; } if ((return_code = device_interface_ptr->RegisterControlLostCallback((gpointer) &ControlLostCallback, this)) < GENERAL_FUNCTION_OK){ - ROS_FATAL_STREAM("Error while registering control lost callback. Code: " << return_code); + RCLCPP_FATAL_STREAM(LOGGER, "Error while registering control lost callback. Code: " << return_code); goto stop_initialization; } @@ -221,23 +232,33 @@ namespace scancontrol_driver } // Advertise topic - publisher = nh.advertise(config_.topic_name, 10); - + publisher = nh_->create_publisher(config_.topic_name, 10); + + using std::placeholders::_1; + using std::placeholders::_2; + // Advertise services - get_feature_srv = private_nh_.advertiseService("get_feature", &ScanControlDriver::ServiceGetFeature, this); - set_feature_srv = private_nh_.advertiseService("set_feature", &ScanControlDriver::ServiceSetFeature, this); - get_resolution_srv = private_nh_.advertiseService("get_resolution", &ScanControlDriver::ServiceGetResolution, this); - set_resolution_srv = private_nh_.advertiseService("set_resolution", &ScanControlDriver::ServiceSetResolution, this); - get_available_resolutions_srv = private_nh_.advertiseService("get_available_resolutions", &ScanControlDriver::ServiceGetAvailableResolutions, this); - invert_z_srv = private_nh_.advertiseService("invert_z", &ScanControlDriver::ServiceInvertZ, this); - invert_x_srv = private_nh_.advertiseService("invert_x", &ScanControlDriver::ServiceInvertX, this); + get_feature_srv = private_nh_->create_service( + "~/get_feature", std::bind(&ScanControlDriver::ServiceGetFeature, this, _1, _2)); + set_feature_srv = private_nh_->create_service( + "~/set_feature", std::bind(&ScanControlDriver::ServiceSetFeature, this, _1, _2)); + get_resolution_srv = private_nh_->create_service( + "~/get_resolution", std::bind(&ScanControlDriver::ServiceGetResolution, this, _1, _2)); + set_resolution_srv = private_nh_->create_service( + "~/set_resolution", std::bind(&ScanControlDriver::ServiceSetResolution, this, _1, _2)); + get_available_resolutions_srv = private_nh_->create_service( + "~/get_available_resolutions", std::bind(&ScanControlDriver::ServiceGetAvailableResolutions, this, _1, _2)); + invert_z_srv = private_nh_->create_service( + "~/invert_z", std::bind(&ScanControlDriver::ServiceInvertZ, this, _1, _2)); + invert_x_srv = private_nh_->create_service( + "~/invert_x", std::bind(&ScanControlDriver::ServiceInvertX, this, _1, _2)); } int ScanControlDriver::SetPartialProfile(int &resolution){ gint32 return_code = 0; // Set profile configuration to partial profile if ((return_code = device_interface_ptr->SetProfileConfig(PARTIAL_PROFILE)) < GENERAL_FUNCTION_OK){ - ROS_WARN_STREAM("Error while setting profile config to PARTIAL_PROFILE. Code: " << return_code); + RCLCPP_WARN_STREAM(LOGGER, "Error while setting profile config to PARTIAL_PROFILE. Code: " << return_code); return GENERAL_FUNCTION_FAILED; } @@ -253,7 +274,7 @@ namespace scancontrol_driver t_partial_profile_.nPointCount = (config_.pp_point_count == -1 || config_.pp_point_count > resolution) ? config_.resolution : config_.pp_point_count; // Send warning and return failed - ROS_WARN_STREAM("Error while setting partial profile settings. Code: " << return_code); + RCLCPP_WARN_STREAM(LOGGER, "Error while setting partial profile settings. Code: " << return_code); return GENERAL_FUNCTION_FAILED; } @@ -262,7 +283,7 @@ namespace scancontrol_driver lost_values = (16 + t_partial_profile_.nPointDataWidth - 1)/t_partial_profile_.nPointDataWidth; value_x.resize(t_partial_profile_.nPointCount); value_z.resize(t_partial_profile_.nPointCount); - ROS_INFO_STREAM("Profile is losing " << std::to_string(lost_values) << " values due to timestamp of 16 byte at the end of the profile."); + RCLCPP_INFO_STREAM(LOGGER, "Profile is losing " << std::to_string(lost_values) << " values due to timestamp of 16 byte at the end of the profile."); // Prepare new point cloud message point_cloud_msg.reset(new point_cloud_t); @@ -281,7 +302,7 @@ namespace scancontrol_driver int ScanControlDriver::StartProfileTransfer(){ int return_code = 0; if ((return_code = device_interface_ptr->TransferProfiles(NORMAL_TRANSFER, true)) < GENERAL_FUNCTION_OK) { - ROS_WARN_STREAM("Error while starting profile transfer! Code: " << return_code); + RCLCPP_WARN_STREAM(LOGGER, "Error while starting profile transfer! Code: " << return_code); return return_code; } transfer_active_ = true; @@ -292,7 +313,7 @@ namespace scancontrol_driver int ScanControlDriver::StopProfileTransfer(){ int return_code = 0; if ((return_code = device_interface_ptr->TransferProfiles(NORMAL_TRANSFER, false)) < GENERAL_FUNCTION_OK) { - ROS_WARN_STREAM("Error while stopping profile transfer. Code: " << return_code); + RCLCPP_WARN_STREAM(LOGGER, "Error while stopping profile transfer. Code: " << return_code); return return_code; } transfer_active_ = false; @@ -320,7 +341,7 @@ namespace scancontrol_driver /* Process and publish profile */ int ScanControlDriver::ProcessAndPublishProfile(const void * data, size_t data_size){ // Timestamp - pcl_conversions::toPCL(ros::Time::now(), point_cloud_msg->header.stamp); + pcl_conversions::toPCL(nh_->get_clock()->now(), point_cloud_msg->header.stamp); // Copy sensor data to local buffer if (data != NULL && data_size == profile_buffer.size()){ @@ -329,7 +350,11 @@ namespace scancontrol_driver // Process buffer and publish point cloud ScanControlDriver::Profile2PointCloud(); - publisher.publish(point_cloud_msg); + // TODO: Is their a better way as long as pcl_ros is not ported to ROS2 + sensor_msgs::msg::PointCloud2 output_msg; + pcl::toROSMsg(*point_cloud_msg, output_msg); + + publisher->publish(output_msg); return GENERAL_FUNCTION_OK; } @@ -338,7 +363,7 @@ namespace scancontrol_driver int ScanControlDriver::GetFeature(unsigned int setting_id, unsigned int *value){ int return_code = 0; if (return_code = device_interface_ptr->GetFeature(setting_id, value) < GENERAL_FUNCTION_OK){ - ROS_WARN_STREAM("Setting could not be retrieved. Code: " << return_code); + RCLCPP_WARN_STREAM(LOGGER, "Setting could not be retrieved. Code: " << return_code); return return_code; } return GENERAL_FUNCTION_OK; @@ -351,87 +376,104 @@ namespace scancontrol_driver int ScanControlDriver::SetFeature(unsigned int setting_id, unsigned int value){ int return_code = 0; if ((std::find(features_with_corruption_risk.begin(), features_with_corruption_risk.end(), setting_id) != features_with_corruption_risk.end()) && transfer_active_){ - ROS_INFO("Risk of profile corruption, temporarily stopping profile transfer."); + RCLCPP_INFO(LOGGER, "Risk of profile corruption, temporarily stopping profile transfer."); if (return_code = ScanControlDriver::StopProfileTransfer() < GENERAL_FUNCTION_OK){ - ROS_WARN_STREAM("Profile transfer could not be stopped. Code: " << return_code); + RCLCPP_WARN_STREAM(LOGGER, "Profile transfer could not be stopped. Code: " << return_code); return -1; } if (return_code = device_interface_ptr->SetFeature(setting_id, value) < GENERAL_FUNCTION_OK){ - ROS_WARN_STREAM("Feature could not be set. Code: " << return_code); + RCLCPP_WARN_STREAM(LOGGER, "Feature could not be set. Code: " << return_code); return return_code; } if (return_code = ScanControlDriver::StartProfileTransfer() < GENERAL_FUNCTION_OK){ - ROS_WARN_STREAM("Profile transfer could not be restarted after changing feature. Code: " << return_code); + RCLCPP_WARN_STREAM(LOGGER, "Profile transfer could not be restarted after changing feature. Code: " << return_code); return -1; } return GENERAL_FUNCTION_OK; } if (return_code = device_interface_ptr->SetFeature(setting_id, value) < GENERAL_FUNCTION_OK){ - ROS_WARN_STREAM("Feature could not be set. Code: " << return_code); + RCLCPP_WARN_STREAM(LOGGER, "Feature could not be set. Code: " << return_code); return return_code; } return GENERAL_FUNCTION_OK; } /* Wrapper of the SetFeature call for use by the ServiceSetFeature service */ - bool ScanControlDriver::ServiceSetFeature(micro_epsilon_scancontrol_msgs::SetFeature::Request &request, micro_epsilon_scancontrol_msgs::SetFeature::Response &response){ - response.return_code = ScanControlDriver::SetFeature(request.setting, request.value); - return true; + void ScanControlDriver::ServiceSetFeature( + const std::shared_ptr request, + std::shared_ptr response) + { + response->return_code = ScanControlDriver::SetFeature(request->setting, request->value); + // return true; } /* Wrapper of the GetFeature call for use by the ServiceGetFeature service */ - bool ScanControlDriver::ServiceGetFeature(micro_epsilon_scancontrol_msgs::GetFeature::Request &request, micro_epsilon_scancontrol_msgs::GetFeature::Response &response){ - response.return_code = ScanControlDriver::GetFeature(request.setting, &(response.value)); - return true; + void ScanControlDriver::ServiceGetFeature( + const std::shared_ptr request, + std::shared_ptr response) + { + response->return_code = ScanControlDriver::GetFeature(request->setting, &(response->value)); + // return true; } /* Wrapper of the SetResolution call for use by the ServiceSetResolution service */ - bool ScanControlDriver::ServiceSetResolution(micro_epsilon_scancontrol_msgs::SetResolution::Request &request, micro_epsilon_scancontrol_msgs::SetResolution::Response &response){ - if (response.return_code = StopProfileTransfer() < GENERAL_FUNCTION_OK){ - return true; + void ScanControlDriver::ServiceSetResolution( + const std::shared_ptr request, + std::shared_ptr response) + { + if (response->return_code = StopProfileTransfer() < GENERAL_FUNCTION_OK){ + // return true; } - if (response.return_code = device_interface_ptr->SetResolution(request.resolution) < GENERAL_FUNCTION_OK){ - ROS_WARN_STREAM("Error while setting device resolution! Code: " << response.return_code); - return true; + if (response->return_code = device_interface_ptr->SetResolution(request->resolution) < GENERAL_FUNCTION_OK){ + RCLCPP_WARN_STREAM(LOGGER, "Error while setting device resolution! Code: " << response->return_code); + // return true; } - int temp_resolution = request.resolution; - if (response.return_code = SetPartialProfile(temp_resolution) < GENERAL_FUNCTION_OK){ - ROS_WARN_STREAM("Error while setting partial profile. Code: " << response.return_code); - return true; + int temp_resolution = request->resolution; + if (response->return_code = SetPartialProfile(temp_resolution) < GENERAL_FUNCTION_OK){ + RCLCPP_WARN_STREAM(LOGGER, "Error while setting partial profile. Code: " << response->return_code); + // return true; } - if (response.return_code = StartProfileTransfer() < GENERAL_FUNCTION_OK){ - return true; + if (response->return_code = StartProfileTransfer() < GENERAL_FUNCTION_OK){ + // return true; } // Change of resolution was succesull - config_.resolution = request.resolution; - return true; + config_.resolution = request->resolution; + // return true; } /* Wrapper of the GetResolution call for use by the ServiceGetResolution service */ - bool ScanControlDriver::ServiceGetResolution(micro_epsilon_scancontrol_msgs::GetResolution::Request &request, micro_epsilon_scancontrol_msgs::GetResolution::Response &response){ - response.return_code = device_interface_ptr->GetResolution(&(response.resolution)); - return true; + void ScanControlDriver::ServiceGetResolution( + const std::shared_ptr request, + std::shared_ptr response) + { + response->return_code = device_interface_ptr->GetResolution(&(response->resolution)); + // return true; } /* Wrapper of the GetResolutions call for use by the ServiceGetAvailableResolutions service */ - bool ScanControlDriver::ServiceGetAvailableResolutions(micro_epsilon_scancontrol_msgs::GetAvailableResolutions::Request &request, micro_epsilon_scancontrol_msgs::GetAvailableResolutions::Response &response){ + void ScanControlDriver::ServiceGetAvailableResolutions( + const std::shared_ptr request, + std::shared_ptr response + ){ guint32 available_resolutions[MAX_RESOLUTION_COUNT] = {0}; - response.return_code = device_interface_ptr->GetResolutions(&available_resolutions[0], MAX_RESOLUTION_COUNT); + response->return_code = device_interface_ptr->GetResolutions(&available_resolutions[0], MAX_RESOLUTION_COUNT); for (int i = 0; i < MAX_RESOLUTION_COUNT; i++){ if (available_resolutions[i] > 0){ - response.resolutions.push_back(available_resolutions[i]); + response->resolutions.push_back(available_resolutions[i]); } } - return true; + // return true; } /* Enable or disable the inversion of Z values on the scanCONTROL device: - If request.data == true > Enable the inversion of Z values on the scanCONTROL device. (Default of the scanCONTROL device) - If request.data == false > Disable the inversion of Z values on the scanCONTROL device. + If request->data == true > Enable the inversion of Z values on the scanCONTROL device. (Default of the scanCONTROL device) + If request->data == false > Disable the inversion of Z values on the scanCONTROL device. */ - bool ScanControlDriver::ServiceInvertZ(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response) + void ScanControlDriver::ServiceInvertZ( + const std::shared_ptr request, + std::shared_ptr response) { unsigned int value; int return_code; @@ -440,14 +482,14 @@ namespace scancontrol_driver if (return_code = ScanControlDriver::GetFeature(FEATURE_FUNCTION_PROCESSING_PROFILEDATA ,&value) < GENERAL_FUNCTION_OK) { // Failed to get PROCESSING feature - response.success = false; - response.message = std::string("Failed to get 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); - return true; + response->success = false; + response->message = std::string("Failed to get 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); + // return true; } // Set 6th bit according to the SetBool service request value = value & ~(1<<6); - if (request.data) + if (request->data) { value |= (1<<6); } @@ -456,22 +498,24 @@ namespace scancontrol_driver if (return_code = ScanControlDriver::SetFeature(FEATURE_FUNCTION_PROCESSING_PROFILEDATA, value) < GENERAL_FUNCTION_OK) { // Failed to set PROCESSING feature - response.success = false; - response.message = std::string("Failed to set 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); - return true; + response->success = false; + response->message = std::string("Failed to set 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); + // return true; } - response.success = true; + response->success = true; - return true; + // return true; } /* Enable or disable the inversion of X values on the scanCONTROL device: - If request.data == true > Enable the inversion of X values on the scanCONTROL device. (Default of the scanCONTROL device) - If request.data == false > Disable the inversion of X values on the scanCONTROL device. + If request->data == true > Enable the inversion of X values on the scanCONTROL device. (Default of the scanCONTROL device) + If request->data == false > Disable the inversion of X values on the scanCONTROL device. */ - bool ScanControlDriver::ServiceInvertX(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response) + void ScanControlDriver::ServiceInvertX( + const std::shared_ptr request, + std::shared_ptr response) { unsigned int value; int return_code; @@ -480,14 +524,14 @@ namespace scancontrol_driver if (return_code = ScanControlDriver::GetFeature(FEATURE_FUNCTION_PROCESSING_PROFILEDATA ,&value) < GENERAL_FUNCTION_OK) { // Failed to get PROCESSING feature - response.success = false; - response.message = std::string("Failed to get 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); - return true; + response->success = false; + response->message = std::string("Failed to get 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); + // return true; } // Set 6th bit according to the SetBool service request value = value & ~(1<<7); - if (request.data) + if (request->data) { value |= (1<<7); } @@ -496,14 +540,14 @@ namespace scancontrol_driver if (return_code = ScanControlDriver::SetFeature(FEATURE_FUNCTION_PROCESSING_PROFILEDATA, value) < GENERAL_FUNCTION_OK) { // Failed to set PROCESSING feature - response.success = false; - response.message = std::string("Failed to set 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); - return true; + response->success = false; + response->message = std::string("Failed to set 'Profile Data Processing' feature. Error code: ") + std::to_string(return_code); + // return true; } - response.success = true; + response->success = true; - return true; + // return true; } /* Callback for when a new profile is read, for use with the scanCONTROL API. */ @@ -517,8 +561,8 @@ namespace scancontrol_driver /* Callback for when connection to the device is lost, for use with the scanCONTROL API. */ void ControlLostCallback(ArvGvDevice *mydevice, gpointer user_data){ - ROS_FATAL("Conrol of scanCONTROL device lost!"); - ros::shutdown(); + RCLCPP_FATAL(LOGGER, "Conrol of scanCONTROL device lost!"); + rclcpp::shutdown(); } } // namespace scancontrol_driver \ No newline at end of file diff --git a/micro_epsilon_scancontrol_driver/src/node.cpp b/micro_epsilon_scancontrol_driver/src/node.cpp index 788c2fa..4547622 100644 --- a/micro_epsilon_scancontrol_driver/src/node.cpp +++ b/micro_epsilon_scancontrol_driver/src/node.cpp @@ -1,31 +1,33 @@ -#include +#include "rclcpp/rclcpp.hpp" #include "micro_epsilon_scancontrol_driver/driver.h" +static const rclcpp::Logger logger = rclcpp::get_logger("scancontrol_driver"); + int main(int argc, char** argv) { - ros::init(argc, argv, "micro_epsilon_scancontrol_driver_node"); - ros::NodeHandle node; - ros::NodeHandle private_nh("~"); - + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("scancontrol_driver"); + rclcpp::Node::SharedPtr private_node = rclcpp::Node::make_shared("scancontrol_driver"); + // Start the driver try { - scancontrol_driver::ScanControlDriver driver(node, private_nh); - ROS_INFO("Driver started"); + scancontrol_driver::ScanControlDriver driver(node, private_node); + RCLCPP_INFO(logger, "Driver started"); // Loop driver until shutdown driver.StartProfileTransfer(); - while(ros::ok()) + while(rclcpp::ok()) { - ros::spinOnce(); + rclcpp::spin_some(node); } driver.StopProfileTransfer(); return 0; } catch(const std::runtime_error& error) { - ROS_FATAL_STREAM(error.what()); - ros::shutdown(); + RCLCPP_FATAL_STREAM(logger, error.what()); + rclcpp::shutdown(); return 0; } diff --git a/micro_epsilon_scancontrol_driver/src/nodelet.cpp b/micro_epsilon_scancontrol_driver/src/nodelet.cpp deleted file mode 100644 index e41e5c5..0000000 --- a/micro_epsilon_scancontrol_driver/src/nodelet.cpp +++ /dev/null @@ -1,30 +0,0 @@ -#include "micro_epsilon_scancontrol_driver/nodelet.h" - -// Register this plugin with pluginlib. -PLUGINLIB_EXPORT_CLASS(scancontrol_driver::DriverNodelet, nodelet::Nodelet) - -namespace scancontrol_driver -{ - - void DriverNodelet::onInit() - { - // Start scanCONTROL driver - driver_.reset(new ScanControlDriver(getNodeHandle(), getPrivateNodeHandle())); - - // spawn device poll thread - // running_ = true; - // device_thread_ = boost::shared_ptr (new boost::thread(boost::bind(&DriverNodelet::devicePoll, this))); - } - - /** @brief Device poll thread main loop. */ - // void DriverNodelet::devicePoll() - // { - // while(ros::ok()) - // { - - // } - // running_ = false; - // } -} - - diff --git a/micro_epsilon_scancontrol_msgs/CMakeLists.txt b/micro_epsilon_scancontrol_msgs/CMakeLists.txt index edd8527..2997947 100644 --- a/micro_epsilon_scancontrol_msgs/CMakeLists.txt +++ b/micro_epsilon_scancontrol_msgs/CMakeLists.txt @@ -1,19 +1,30 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(micro_epsilon_scancontrol_msgs) -find_package(catkin REQUIRED COMPONENTS std_msgs message_generation) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -add_service_files(DIRECTORY srv - FILES - GetAvailableResolutions.srv - SetResolution.srv - GetResolution.srv - SetFeature.srv - GetFeature.srv -) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) -generate_messages(DEPENDENCIES std_msgs) +set(srv_files + "srv/GetAvailableResolutions.srv" + "srv/GetFeature.srv" + "srv/GetResolution.srv" + "srv/SetFeature.srv" + "srv/SetResolution.srv" +) -catkin_package( - CATKIN_DEPENDS message_runtime +rosidl_generate_interfaces(${PROJECT_NAME} + ${srv_files} + DEPENDENCIES + builtin_interfaces + std_msgs ) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() \ No newline at end of file diff --git a/micro_epsilon_scancontrol_msgs/package.xml b/micro_epsilon_scancontrol_msgs/package.xml index 525ea14..d96d3c7 100644 --- a/micro_epsilon_scancontrol_msgs/package.xml +++ b/micro_epsilon_scancontrol_msgs/package.xml @@ -1,19 +1,26 @@ - + micro_epsilon_scancontrol_msgs - 0.0.1 - Micro Espsilon scanCONTROL Driver Messages + 0.1.0 + Micro Espsilon scanCONTROL driver messages - Rik Tonnear Dave Kroezen - https://gitlab.tudelft.nl/dasml/micro_epsilon_scancontrol + https://github.com/sam-xl/scancontrol - BSD + Apache 2.0 - catkin + ament_cmake - message_generation + rosidl_default_generators - message_runtime + std_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake +