diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..817fd6d --- /dev/null +++ b/.clang-format @@ -0,0 +1,65 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: true +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} +... diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 0000000..cd57f27 --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,106 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ifm3d-ros +^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0 +=== + +1.0.0 +------ + +Braking changes: ++ Restructure the ifm3d-ros package into independent subpackages. Please check your path declarations again, especially for the launch files and messages and services. + +Changes between ifm3d_ros 0.6.x and 1.0.0: ++ Order of axis changed in 3D (cloud topic and extrinsic calibration parameters): This wrapper keeps the axis orientation as defined by the underlying API, ifm3d. Therefore, you may see a different axis order for the cloud message compared to older versions of the ifm3d and ifm cameras. ++ Extrinsic calibration parameters: now conistent with SI units, e.g. translation are scaled in `m` and rotation parameters are scaled in `rad`. ++ Added publisher for 2D RGB data ++ Use CameraBase for compatibility with other O3 devices ++ Comment out methods / publisher which are not available for the O3RCamera (at the moment) ++ Comment out the unit vector publishing ++ Changed services trigger and softon, softoff to be compatible with new JSON methods and schema. ++ Changed service trigger to a dummy method until triggers are implemented for the O3R platform. It only has a status message at the moment. ++ Changed service dump: coverts from json to str for displaying the message + +added: ++ Added pcic_port to the list of framegrabber arguments + +known limitations: ++ This version of the ifm3d-ros package only works with the O3R camera platform. + + +0.6 +=== + +0.6.2 +----- +Changes between ifm3d_ros 0.6.1 and 0.6.2 + +* Updated maintainer email address +* Added ifm3d-core dependency in preparation for submission to the ROS index + +0.6.1 +----- + +* Added support syncing the system and camera clocks at startup. Side-effect, + is we can now stamp the images with the camera-side capture time and not the + host-side reception time. +* Added the `SyncClocks` Service + +0.6.0. +------ + +* Added a image transport plugin _blacklist_ to the nodlet launch file. This + prevents many of the errors seen in the terminal when running `rosbag -a` to + capture camera data +* Added the `SoftOn` and `SoftOff` service calls + +0.5 +=== + + +0.5.1 +----- + +* Added support for Ubuntu 18.04 and ROS Melodic + +0.5.0 +----- + +* Converted primary data publisher to a nodelet architecture +* Provide the `dump` and `config` scripts to call into the exposed ROS services + of the nodelet. Removed the older "config node". +* Added unit tests + +0.4 +=== + +0.4.2 +----- + +* Now requires ifm3d 0.9.0 and by association the more modernized tooling + (C++14, cmake 3.5, dropped support for 14.04/Indigo, etc.) + +0.4.1 +----- + +* Now publishing extrinsics on a topic + +0.3 +=== + +* Added `Dump` Service +* Added `Config` Service +* Added `Trigger` Service + +0.2 +=== + +* Updates to CMakeLists.txt to support Ubuntu 14.04 and ROS Indigo + +0.1 +=== + +This file has started tracking ifm3d_ros at 0.1.0 + +* Initial (alpha) release diff --git a/ChangeLog.md b/ChangeLog.md deleted file mode 100644 index ceb694a..0000000 --- a/ChangeLog.md +++ /dev/null @@ -1,50 +0,0 @@ -## Changes between ifm3d-ros 0.6.1 and 0.6.2 - -* Updated maintainer email address -* Added ifm3d-core dependency in preparation for submission to the ROS index - -## Changes between ifm3d-ros 0.6.0 and 0.6.1 - -* Added support syncing the system and camera clocks at startup. Side-effect, - is we can now stamp the images with the camera-side capture time and not the - host-side reception time. -* Added the `SyncClocks` Service - -## Changes between ifm3d-ros 0.5.1 and 0.6.0 - -* Added a image transport plugin _blacklist_ to the nodlet launch file. This - prevents many of the errors seen in the terminal when running `rosbag -a` to - capture camera data -* Added the `SoftOn` and `SoftOff` service calls - -## Changes between ifm3d-ros 0.5.0 and 0.5.1 - -* Added support for Ubuntu 18.04 and ROS Melodic - -## Changes between ifm3d-ros 0.4.2 and 0.5.0 -* Converted primary data publisher to a nodelet architecture -* Provide the `dump` and `config` scripts to call into the exposed ROS services - of the nodelet. Removed the older "config node". -* Added unit tests - -## Changes between ifm3d-ros 0.4.1 and 0.4.2 -* Now requires ifm3d 0.9.0 and by association the more modernized tooling - (C++14, cmake 3.5, dropped support for 14.04/Indigo, etc.) - -## Changes between ifm3d-ros 0.3.0 and 0.4.0 - -* Now publishing extrinsics on a topic - -## Changes between ifm3d-ros 0.2.0 and 0.3.0 - -* Added `Dump` Service -* Added `Config` Service -* Added `Trigger` Service - -## Changes between ifm3d-ros 0.1.0 and 0.2.0 - -* Updates to CMakeLists.txt to support Ubuntu 14.04 and ROS Indigo - -## This file has started tracking ifm3d-ros at 0.1.0 - -* Initial (alpha) release diff --git a/README.md b/README.md index ee9c3b8..9dfbc46 100644 --- a/README.md +++ b/README.md @@ -1,408 +1,46 @@ -ifm3d-ros -========= -ifm3d-ros is a wrapper around [ifm3d](https://github.com/lovepark/ifm3d) -enabling the usage of ifm pmd-based ToF cameras from within -[ROS](http://ros.org) software systems. +# ifm3d-ros wrapper around the ifm3d library -![rviz1](doc/figures/rviz_sample.png) +# Release versions -Software Compatibility Matrix -============================= - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
ifm3d-ros versionifm3d versionROS distribution(s)
0.1.00.1.0Kinetic
0.2.00.2.0Kinetic, Indigo
0.3.00.2.0Kinetic, Indigo
0.4.00.2.0, 0.3.0, 0.3.1, 0.3.2Kinetic, Indigo
0.4.10.3.3, 0.4.0, 0.5.0, 0.6.0, 0.7.0, 0.8.1Kinetic, Indigo
0.4.20.9.0Kinetic
0.5.00.9.0, 0.9.1Kinetic
0.5.10.9.2Kinetic, Melodic
0.6.00.9.2, 0.9.3, 0.10.0, 0.11.0, 0.11.2Kinetic, Melodic
0.6.1, 0.6.20.11.2, 0.12.0, 0.17.0Kinetic, Melodic
+:warning: Note that the `v1.0.x` branch is generally in a work in progress state, and you probably want to use a tagged [release version](https://github.com/ifm/ifm3d-ros/releases) for production. -Building and Installing the Software -==================================== +## ifm3d-ros for the O3R -1. Preparing your system: [Kinetic](doc/kinetic.md), [Melodic](doc/melodic.md) -2. [Installing the ROS node](doc/building.md) +**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. -ROS Interface -============= -## camera nodelet +ifm3d-ros is a wrapper around [ifm3d](https://github.com/ifm/ifm3d/) enabling the usage of the O3R camera platform (ifm ToF cameras) from within [ROS](http://ros.org) software systems. -The core `ifm3d-ros` sensor interface is implemented as a ROS nodelet. This -allows for lower-latency data processing vs. the traditional out-of-process -node-based ROS interface for applications that require it. However, we ship a -launch file with this package that allows for using the core `ifm3d-ros` driver -as a standard node. To launch the node, the following command can be used: +![rviz1](ifm3d_ros_driver/doc/figures/O3R_merged_point_cloud.png) -``` -$ roslaunch ifm3d camera.launch -``` +## Software Compatibility Matrix -We note, the above command is equivalent to the following and is used for -purposes of providing a backward compatible interface to versions of -`ifm3d-ros` prior to the conversion to a nodelet architecture: +|**ifm3d-ros version**|**ifm3d version**|**ROS distribution(s)**| +| ------------ | ------------ | ------------ | +| 1.0.0 | in development - latest version checked 0.91.0 | Noetic | -``` -$ roslaunch ifm3d nodelet.launch __ns:=ifm3d -``` +### Internal ifm3d-ros subpackage version structure +Please see the internal subpackage version structure for a known `ifm3d-ros version`. -Regardless of which command line you used from above, the launch file(s) -encapsulate several features: +|**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. It exposes some of the `camera_nodelet` parameters as command-line arguments - for ease of runtime configuration. -2. It instantiates a nodelet manager which the `camera_nodelet` will be loaded - into. -3. It launches the camera nodelet itself. -4. It publishes the static transform from the camera's optical frame to a - traditional ROS sensor frame as a tf2 `static_transform_publisher`. +## Organization of the software -You can either use [this launch file](launch/camera.launch) directly, or, use -it as a basis for integrating `ifm3d-ros` into your own robot software system. +The `ifm3d-ros` meta package provides three subpackages: +- `ifm3d_ros_driver` provides the core interface for receiving data for ifm 3d (O3R) cameras. +- `ifm3d_ros_msgs` gathers the ifm-specific messages types and the services for configuring and triggering the camera. +- `ifm3d_ros_examples` provides additional helper scripts and examples. -We note: due to the change in architecture from a standalone node to a nodelet, -we have seen one behavior whose solution is not clear to have us provide -backward compatible behavior with older versions of `ifm3d-ros`. Specifically, -if you plan to run the camera in software triggered mode, you should lanch the -node as follows: +The name `ifm3d-ros` was kept even tough this is not consistent with ROS package naming conventions. +This ROS package has been split into three sub packages in an effort to facilitate dependency handling on distributed systems and simplify deployment on embedded platforms. For instance, the package `ifm3d_ros_msgs` can be installed independently of the other packages to control the camera from a separate computing platform. The `ifm3d_ros_examples` holds our launch files and examples. -``` -$ GLOG_minloglevel=3 roslaunch ifm3d camera.launch assume_sw_triggered:=true -``` +## Building and installing the software -The incomatibility here is that in prior versions, one would not need to -explicitly set the `GLOG_` environment variable. While not strictly necessary, -it is recommended for keeping the noise level of the `ifm3d` logs low. +1. Preparing your system: [Noetic](ifm3d_ros_driver/doc/noetic.md) +2. [Installing the ifm3d-ros node](ifm3d_ros_driver/doc/building.md) - -### Parameters - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
NameData TypeDefault ValueDescription
~assume_sw_triggeredboolfalse - This provides a hint to the driver that the camera is configured for - software triggering (as opposed to free running). In this mode, certain - default values are applied to lessen the noise in terms of timeouts from - the framegrabber. -
~frame_id_basestringifm3d/camera - This string provides a prefix into the `tf` tree for `ifm3d-ros` - coordinate frames. -
~frame_latency_threshfloat60.0 - Time (seconds) used to determine that timestamps from the camera cannot - be trusted. When this threshold is exceeded, when compared to system - time, we use the reception time of the frame and not the capture time of - the frame. -
~ipstring192.168.0.69 - The ip address of the camera. -
~passwordstring - The password required to establish an edit session with the camera. -
~schema_maskuint160xf - The pcic schema mask to apply to the active session with the frame - grabber. This determines which images are available for publication from - the camera. More about pcic schemas can be gleaned from the - ifm3d project. -
~timeout_millisint500 - The number of milliseconds to wait for the framegrabber to return new - frame data before declaring a "timeout" and to stop blocking on new - data. -
~timeout_tolerance_secsfloat5.0 - The wall time to wait with no new data from the camera before trying to - establish a new connection to the camera. This helps to provide - robustness against camera cables becoming unplugged or other in-field - pathologies which would cause the connection between the ROS node and the - camera to be broken. -
~soft_on_timeout_millisint500 - If using the `SoftOn` service call, when turning the camera back `on` - this is the setting that will be used for `timeout_millis`. -
~soft_on_timeout_tolerance_secsfloat5.0 - If using the `SoftOn` service call, when turning the camera back `on` - this is the setting that will be used for `timeout_tolerance_secs`. -
~soft_off_timeout_millisint500 - If using the `SoftOff` service call, when turning the camera `off` - this is the setting that will be used for `timeout_millis`. -
~soft_off_timeout_tolerance_secsfloat600.0 - If using the `SoftOff` service call, when turning the camera `off` - this is the setting that will be used for `timeout_tolerance_secs`. -
~sync_clocksboolfalse - Attempt to sync the camera clock to the system clock at start-up. The - side-effect is that timestamps on the image should reflect the capture - time as opposed to the receipt time. -
~xmlrpc_portuint1680 - The TCP port the camera's xmlrpc server is listening on for requests. -
- -### Published Topics - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
NameData TypeDescription
amplitudesensor_msgs/ImageThe normalized amplitude image
cloudsensor_msgs/PointCloud2The point cloud data
confidencesensor_msgs/ImageThe confidence image
distancesensor_msgs/ImageThe radial distance image
extrinsicsifm3d/Extrinsics - The extrinsic calibration of the camera with respect to the camera - optical frame. The data are mm and degrees. -
good_bad_pixelssensor_msgs/Image - A binary image indicating good vs. bad pixels as gleaned from the - confidence data. -
raw_amplitudesensor_msgs/ImageThe raw amplitude image
unit_vectorssensor_msgs/ImageThe rotated unit vectors
xyz_imagesensor_msgs/Image - A 3-channel image encoding of the point cloud. Each of the three image - channels respesent a spatial data plane encoding the x, y, z cartesian - values respectively. -
- -### Subscribed Topics - -None. - -### Advertised Services - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
NameService DefinitionDescription
Dumpifm3d/DumpDumps the state of the camera parameters to JSON
Configifm3d/Config - Provides a means to configure the camera and imager settings, - declaratively from a JSON encoding of the desired settings. -
SoftOffifm3d/SoftOff - Sets the active application of the camera into software triggered - mode which will turn off the active illumination reducing both power and - heat. -
SoftOnifm3d/SoftOn - Sets the active application of the camera into free-running mode. - Its intention is to act as the inverse of `SoftOff`. -
SyncClocksifm3d/SyncClocks - Synchronizes the camera clock to the system time. The side-effect is that - images can be stamped with the capture time of the frame as opposed to - the reception time. -
Triggerifm3d/Trigger - Requests the driver to software trigger the imager for data - acquisition. -
- - -Additional Documentation -======================== - -* [Inspecting and configuring the camera/imager settings](doc/dump_and_config.md) -* [Troubleshooting](doc/troubleshooting.md) - -LICENSE -------- - -Please see the file called [LICENSE](LICENSE). +# LICENSE +Please see the file called [LICENSE](LICENSE). \ No newline at end of file diff --git a/bin/config b/bin/config deleted file mode 100755 index 1e2ba9b..0000000 --- a/bin/config +++ /dev/null @@ -1,24 +0,0 @@ -#!/usr/bin/env python -# -*- python -*- - -# -# Copyright (C) 2018 ifm electronic, gmbh -# -# 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 distribted 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. -# - -import sys -from ifm3d import ConfigClient - -if __name__ == '__main__': - sys.exit(ConfigClient().run()) diff --git a/bin/dump b/bin/dump deleted file mode 100755 index 9d4c8b0..0000000 --- a/bin/dump +++ /dev/null @@ -1,24 +0,0 @@ -#!/usr/bin/env python -# -*- python -*- - -# -# Copyright (C) 2018 ifm electronic, gmbh -# -# 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 distribted 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. -# - -import sys -from ifm3d import DumpClient - -if __name__ == '__main__': - sys.exit(DumpClient().run()) diff --git a/clean_emacs.sh b/clean_emacs.sh deleted file mode 100755 index 1e3781b..0000000 --- a/clean_emacs.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# Recursively removes emacs backup files from your source directory. -# -echo "Cleaning up emacs backup files..." -find . -name "*~" | xargs rm -f -echo "OK" \ No newline at end of file diff --git a/doc/building.md b/doc/building.md deleted file mode 100644 index cc55955..0000000 --- a/doc/building.md +++ /dev/null @@ -1,124 +0,0 @@ - -Building and Installing the ifm3d-ros Package ---------------------------------------------- - -### Prerequisites - -1. [Ubuntu 16.04 LTS](http://www.ubuntu.com) -2. [ROS Kinetic](http://www.ros.org/install) - we recommend `ros-kinetic-desktop-full`. -3. [ifm3d](https://github.com/lovepark/ifm3d) - version >= 0.9.0 - -or - -1. [Ubuntu 18.04 LTS](http://www.ubuntu.com) -2. [ROS Melodic](http://www.ros.org/install) - we recommend `ros-melodic-desktop-full`. -3. [ifm3d](https://github.com/lovepark/ifm3d) - version >= 0.9.2 - -### Step-by-Step Build Instructions - -Step-by-step instructions on getting a fresh installation of Ubuntu and ROS -prepared for usage with `ifm3d-ros` is available at the following links: -* [Ubuntu 16.04 with ROS Kinetic](kinetic.md) -* [Ubuntu 18.04 with ROS Melodic](melodic.md) - -Building and installing ifm3d-ros is accomplished by utilizing the ROS -[catkin](http://wiki.ros.org/catkin) tool. There are many tutorials and other -pieces of advice available on-line advising how to most effectively utilize -catkin. However, the basic idea is to provide a clean separation between your -source code repository and your build and runtime environments. The -instructions that now follow represent how we choose to use catkin to build and -_permanently install_ a ROS package from source. - -First, we need to decide where we want our software to ultimately be -installed. For purposes of this document, we will assume that we will install -our ROS packages at `~/ros`. For convenience, we add the following to our -`~/.bashrc`: - -NOTE: Below we assume `kinetic`. Adapting to `melodic` is left as an exercise -for the reader. - -``` -if [ -f /opt/ros/kinetic/setup.bash ]; then - source /opt/ros/kinetic/setup.bash -fi - -cd ${HOME} - -export LPR_ROS=${HOME}/ros - -if [ -d ${LPR_ROS} ]; then - for i in $(ls ${LPR_ROS}); do - if [ -d ${LPR_ROS}/${i} ]; then - if [ -f ${LPR_ROS}/${i}/setup.bash ]; then - source ${LPR_ROS}/${i}/setup.bash --extend - fi - fi - done -fi -``` - -Next, we need to get the code from github. We assume we keep all of our git -repositories in `~/dev`. - -``` -$ cd ~/dev -$ git clone https://github.com/lovepark/ifm3d-ros.git -``` - -We now have the code in `~/dev/ifm3d-ros`. Next, we want to create a _catkin -workspace_ that we can use to build and install that code from. It is the -catkin philosophy that we do not do this directly in the source directory. - -``` -$ cd ~/catkin -$ mkdir ifm3d -$ cd ifm3d -$ mkdir src -$ cd src -$ catkin_init_workspace -$ ln -s ~/dev/ifm3d-ros ifm3d -``` - -So, you should have a catkin workspace set up to build the ifm3d-ros code that -looks basically like: - -``` -[ ~/catkin/ifm3d/src ] -tpanzarella@tuna: $ pwd -/home/tpanzarella/catkin/ifm3d/src - -[ ~/catkin/ifm3d/src ] -tpanzarella@tuna: $ ls -l -total 0 -lrwxrwxrwx 1 tpanzarella tpanzarella 50 Mar 26 15:16 CMakeLists.txt -> /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake -lrwxrwxrwx 1 tpanzarella tpanzarella 31 Mar 26 15:16 ifm3d -> /home/tpanzarella/dev/ifm3d-ros -``` - -Now we are ready to build the code. - -``` -$ cd ~/catkin/ifm3d -$ catkin_make -DCATKIN_ENABLE_TESTING=ON -$ catkin_make run_tests -$ catkin_make -DCMAKE_INSTALL_PREFIX=${LPR_ROS}/ifm3d install -``` - -The ROS package should now be installed in `~/ros/ifm3d`. To test everything -out you should open a fresh bash shell, and start up a ROS core: - - $ roscore - -Open another shell and start the primary camera node: - - $ roslaunch ifm3d camera.launch - -Open another shell and start the rviz node to visualize the data coming from -the camera: - - $ roslaunch ifm3d rviz.launch - -At this point, you should see an rviz window that looks something like: - -![rviz1](figures/rviz_sample.png) - -Congratulations! You can now utilize ifm3d-ros. diff --git a/doc/dump_and_config.md b/doc/dump_and_config.md deleted file mode 100644 index fd14135..0000000 --- a/doc/dump_and_config.md +++ /dev/null @@ -1,183 +0,0 @@ - -ifm3d-ros: Dump and Config -========================== - -`ifm3d-ros` provides access to the camera/imager parameters via the `Dump` and -`Config` services exposed by the `camera_nodelet`. Additionally, command-line -scripts called `dump` and `config` are provided as wrapper interfaces to those -services. This gives a feel similar to using the underlying `ifm3d` -command-line tool from the ROS-independent driver except proxying the calls -through the ros network. - -For example, to dump the state of the camera: - -(exemplary output from an O3D303 is shown) - -``` -$ rosrun ifm3d dump -{ - "ifm3d": { - "Apps": [ - { - "Description": "", - "Id": "322089668", - "Imager": { - "AutoExposureMaxExposureTime": "10000", - "AutoExposureReferencePointX": "88", - "AutoExposureReferencePointY": "66", - "AutoExposureReferenceROI": "{\"ROIs\":[{\"id\":0,\"group\":0,\"type\":\"Rect\",\"width\":130,\"height\":100,\"angle\":0,\"center_x\":88,\"center_y\":66}]}", - "AutoExposureReferenceType": "0", - "Channel": "0", - "ClippingBottom": "131", - "ClippingCuboid": "{\"XMin\": -3.402823e+38, \"XMax\": 3.402823e+38, \"YMin\": -3.402823e+38, \"YMax\": 3.402823e+38, \"ZMin\": -3.402823e+38, \"ZMax\": 3.402823e+38}", - "ClippingLeft": "0", - "ClippingRight": "175", - "ClippingTop": "0", - "ContinuousAutoExposure": "false", - "ContinuousUserFrameCalibration": "false", - "EnableAmplitudeCorrection": "true", - "EnableFastFrequency": "false", - "EnableFilterAmplitudeImage": "true", - "EnableFilterDistanceImage": "true", - "EnableRectificationAmplitudeImage": "false", - "EnableRectificationDistanceImage": "false", - "ExposureTime": "5000", - "ExposureTimeList": "125;5000", - "ExposureTimeRatio": "40", - "FrameRate": "10", - "MaxAllowedLEDFrameRate": "23.2", - "MinimumAmplitude": "42", - "Resolution": "0", - "SpatialFilter": {}, - "SpatialFilterType": "0", - "SymmetryThreshold": "0.4", - "TemporalFilter": {}, - "TemporalFilterType": "0", - "ThreeFreqMax2FLineDistPercentage": "80", - "ThreeFreqMax3FLineDistPercentage": "80", - "TwoFreqMaxLineDistPercentage": "80", - "Type": "under5m_moderate", - "UseSimpleBinning": "false" - }, - "Index": "1", - "LogicGraph": "{\"IOMap\": {\"OUT1\": \"RFT\",\"OUT2\": \"AQUFIN\"},\"blocks\": {\"B00001\": {\"pos\": {\"x\": 200,\"y\": 200},\"properties\": {},\"type\": \"PIN_EVENT_IMAGE_ACQUISITION_FINISHED\"},\"B00002\": {\"pos\": {\"x\": 200,\"y\": 75},\"properties\": {},\"type\": \"PIN_EVENT_READY_FOR_TRIGGER\"},\"B00003\": {\"pos\": {\"x\": 600,\"y\": 75},\"properties\": {\"pulse_duration\": 0},\"type\": \"DIGITAL_OUT1\"},\"B00005\": {\"pos\": {\"x\": 600,\"y\": 200},\"properties\": {\"pulse_duration\": 0},\"type\": \"DIGITAL_OUT2\"}},\"connectors\": {\"C00000\": {\"dst\": \"B00003\",\"dstEP\": 0,\"src\": \"B00002\",\"srcEP\": 0},\"C00001\": {\"dst\": \"B00005\",\"dstEP\": 0,\"src\": \"B00001\",\"srcEP\": 0}}}", - "Name": "Sample Application", - "PcicEipResultSchema": "{ \"layouter\": \"flexible\", \"format\": { \"dataencoding\": \"binary\", \"order\": \"big\" }, \"elements\" : [ { \"type\": \"string\", \"value\": \"star\", \"id\": \"start_string\" }, { \"type\": \"records\", \"id\": \"models\", \"elements\": [ { \"type\": \"int16\", \"id\": \"boxFound\" }, { \"type\": \"int16\", \"id\": \"width\", \"format\": { \"scale\": 1000 } }, { \"type\": \"int16\", \"id\": \"height\", \"format\": { \"scale\": 1000 } }, { \"type\": \"int16\", \"id\": \"length\", \"format\": { \"scale\": 1000 } }, { \"type\": \"int16\", \"id\": \"xMidTop\", \"format\": { \"scale\": 1000 } }, { \"type\": \"int16\", \"id\": \"yMidTop\", \"format\": { \"scale\": 1000 } }, { \"type\": \"int16\", \"id\": \"zMidTop\", \"format\": { \"scale\": 1000 } }, { \"type\": \"int16\", \"id\": \"yawAngle\" }, { \"type\": \"int16\", \"id\": \"qualityLength\" }, { \"type\": \"int16\", \"id\": \"qualityWidth\" }, { \"type\": \"int16\", \"id\": \"qualityHeight\" } ] }, { \"type\": \"string\", \"value\": \"stop\", \"id\": \"end_string\" } ] }", - "PcicPnioResultSchema": "{\"layouter\" : \"flexible\", \"format\": { \"dataencoding\": \"binary\", \"order\": \"big\" }, \"elements\" : [ { \"type\": \"string\", \"value\": \"star\", \"id\": \"start_string\" }, { \"type\": \"records\", \"id\": \"models\", \"elements\": [ { \"type\": \"int16\", \"id\": \"boxFound\" }, { \"type\": \"int16\", \"id\": \"width\", \"format\": { \"scale\": 1000 } }, { \"type\": \"int16\", \"id\": \"height\", \"format\": { \"scale\": 1000 } }, { \"type\": \"int16\", \"id\": \"length\", \"format\": { \"scale\": 1000 } }, { \"type\": \"int16\", \"id\": \"xMidTop\", \"format\": { \"scale\": 1000 } }, { \"type\": \"int16\", \"id\": \"yMidTop\", \"format\": { \"scale\": 1000 } }, { \"type\": \"int16\", \"id\": \"zMidTop\", \"format\": { \"scale\": 1000 } }, { \"type\": \"int16\", \"id\": \"yawAngle\" }, { \"type\": \"int16\", \"id\": \"qualityLength\" }, { \"type\": \"int16\", \"id\": \"qualityWidth\" }, { \"type\": \"int16\", \"id\": \"qualityHeight\" } ] }, { \"type\": \"string\", \"value\": \"stop\", \"id\": \"end_string\" } ] }", - "PcicTcpResultSchema": "{ \"layouter\": \"flexible\", \"format\": { \"dataencoding\": \"ascii\" }, \"elements\": [ { \"type\": \"string\", \"value\": \"star\", \"id\": \"start_string\" }, { \"type\": \"blob\", \"id\": \"normalized_amplitude_image\" }, { \"type\": \"blob\", \"id\": \"distance_image\" }, { \"type\": \"blob\", \"id\": \"x_image\" }, { \"type\": \"blob\", \"id\": \"y_image\" }, { \"type\": \"blob\", \"id\": \"z_image\" }, { \"type\": \"blob\", \"id\": \"confidence_image\" }, { \"type\": \"blob\", \"id\": \"diagnostic_data\" }, { \"type\": \"string\", \"value\": \"stop\", \"id\": \"end_string\" } ] }", - "RtspOverlayStyle": "{\"ROI\": {\"default\": {\"visible\": true, \"filled\": false, \"use_symbol\": false, \"label_alignment\": \"top\", \"label_content\": \"\", \"label_background\": \"black\", \"label_fontsize\": 8, \"label_failonly\": false}, \"model_defaults\": {}, \"specific\": {} } }", - "TemplateInfo": "", - "TriggerMode": "1", - "Type": "Camera" - } - ], - "Device": { - "ActiveApplication": "1", - "ArticleNumber": "O3D303", - "ArticleStatus": "AD", - "Description": "", - "DeviceType": "1:2", - "EIPConsumingSize": "8", - "EIPProducingSize": "450", - "EnableAcquisitionFinishedPCIC": "false", - "EthernetFieldBus": "1", - "EthernetFieldBusEndianness": "0", - "EvaluationFinishedMinHoldTime": "10", - "ExtrinsicCalibRotX": "0", - "ExtrinsicCalibRotY": "0", - "ExtrinsicCalibRotZ": "0", - "ExtrinsicCalibTransX": "0", - "ExtrinsicCalibTransY": "0", - "ExtrinsicCalibTransZ": "0", - "IODebouncing": "true", - "IOExternApplicationSwitch": "0", - "IOLogicType": "1", - "IPAddressConfig": "0", - "ImageTimestampReference": "1524246144", - "Name": "FooBarBaz", - "OperatingMode": "0", - "PNIODeviceName": "", - "PasswordActivated": "false", - "PcicProtocolVersion": "3", - "PcicTcpPort": "50010", - "PcicTcpSchemaAutoUpdate": "false", - "SaveRestoreStatsOnApplSwitch": "true", - "ServiceReportFailedBuffer": "15", - "ServiceReportPassedBuffer": "15", - "SessionTimeout": "30", - "TemperatureFront1": "3276.7", - "TemperatureFront2": "3276.7", - "TemperatureIMX6": "53.1839981079102", - "TemperatureIllu": "60.6", - "UpTime": "3.13861111111111" - }, - "Net": { - "MACAddress": "00:02:01:40:7D:96", - "NetworkSpeed": "0", - "StaticIPv4Address": "192.168.0.69", - "StaticIPv4Gateway": "192.168.0.201", - "StaticIPv4SubNetMask": "255.255.255.0", - "UseDHCP": "false" - }, - "Time": { - "CurrentTime": "1524246142", - "NTPServers": "", - "StartingSynchronization": "false", - "Stats": "", - "SynchronizationActivated": "false", - "Syncing": "false", - "WaitSyncTries": "2" - }, - "_": { - "Date": "Tue May 15 13:38:32 2018", - "HWInfo": { - "Connector": "#!02_A300_C40_02452814_008023176", - "Diagnose": "#!02_D322_C32_03038544_008023267", - "Frontend": "#!02_F342_C34_17_00049_008023607", - "Illumination": "#!02_I300_001_03106810_008001175", - "MACAddress": "00:02:01:40:7D:96", - "Mainboard": "#!02_M381_003_03181504_008022954", - "MiraSerial": "0e30-59af-0ef7-0244" - }, - "SWVersion": { - "Algorithm_Version": "2.1.5", - "Calibration_Device": "00:02:01:40:7d:96", - "Calibration_Version": "0.9.0", - "Diagnostic_Controller": "v1.0.33-92f88d349f-dirty", - "IFM_Recovery": "unversioned", - "IFM_Software": "1.23.1522", - "Linux": "Linux version 3.14.34-rt31-yocto-standard-00009-ge4ab4d94f288-dirty (jenkins@dettlx190) (gcc version 4.9.2 (GCC) ) #1 SMP PREEMPT RT Tue Mar 13 16:06:07 CET 2018", - "Main_Application": "1.0.33" - }, - "ifm3d_version": 900 - } - } -} -``` - -Chaining together Linux pipelines works just as it does in `ifm3d`. For -example, using a combination of `dump` and `config` to set a new name on the -camera would look like: - -``` -$ rosrun ifm3d dump | jq '.ifm3d.Device.Name="My 3D Camera"' | rosrun ifm3d config -$ rosrun ifm3d dump | jq .ifm3d.Device.Name -"My 3D Camera" -``` - -**NOTE:** If you do not have `jq` on your system, it can be installed with: `$ sudo apt-get install jq` - -For the `config` command, one difference between our ROS implementation and the -`ifm3d` implementation is that we only accept input on `stdin`. So, if you had -a file with JSON you wish to configure your camera with, you would simply use the -file I/O redirection facilities of your shell (or something like `cat`) to feed -the data to `stdin`. For example, in `bash`: - -``` -$ rosrun ifm3d config < camera.json -``` - -Beyond the requirement of prefacing your command-line with `rosrun` to invoke -the ROS version of these tools, they operate the same. To learn more about the -functionality and concepts, you can read the docs -[here](https://github.com/lovepark/ifm3d/blob/master/doc/configuring.md). diff --git a/doc/figures/rviz_sample.png b/doc/figures/rviz_sample.png deleted file mode 100644 index d76f6b6..0000000 Binary files a/doc/figures/rviz_sample.png and /dev/null differ diff --git a/doc/indigo.md b/doc/indigo.md deleted file mode 100644 index 3bfd391..0000000 --- a/doc/indigo.md +++ /dev/null @@ -1,65 +0,0 @@ -ifm3d-ros on Ubuntu 14.04 and ROS Indigo ----------------------------------------- - -This article provides a quick-start guide for getting a fresh install of Ubuntu -14.04 ready for usage with `ifm3d-ros` and an O3X camera. As a pre-requisite -for this article, we assume you already have Ubuntu 14.04 installed (but have -done no other configuration) and that you will be using `ifm3d` version `0.2.0` -and `ifm3d-ros` version `0.2.0`. Later versions of the `ifm3d` and `ifm3d-ros` -software will work as well until we end-of-life Ubuntu 14.04 and ROS -Indigo. Finally, since we assume you have done no updates to your Ubuntu -install, we also assume you are using `g++` version `4.8.4` (which is old). - -### Update the Baseline Packages of your Ubuntu 14.04 Install - -``` -$ sudo apt-get update -$ sudo apt-get -u upgrade -``` - -### Install ROS Indigo - -You should now follow -[these steps](http://wiki.ros.org/indigo/Installation/Ubuntu) exactly (we -assume you did) and that you chose to install `ros-indigo-desktop-full`. Go do -that now, then continue on. - -### Additional Dependencies - -There are a few things that we need to install to successfully build from -source that we did not get implicitly by installing ROS. The following commands -will handle these pre-requisites: - -``` -$ sudo apt-get install libxmlrpc-c++8-dev -$ sudo apt-get install libgoogle-glog-dev -``` - -### Install ifm3d - -[ifm3d](https://github.com/lovepark/ifm3d) is the core underlying C++ driver -that `ifm3d-ros` wraps. We need to install that now. We assume you keep all of -your source code in `~/dev`. - -``` -$ mkdir ~/dev -$ cd ~/dev -$ git clone https://github.com/lovepark/ifm3d.git -$ cd ifm3d -$ mkdir build -$ cd build -$ cmake -DCMAKE_INSTALL_PREFIX=/usr -DFORCE_OPENCV2=ON .. -$ make -$ make check -$ make package -$ make repackage -$ sudo dpkg -i ifm3d_0.2.0_amd64-camera.deb -$ sudo dpkg -i ifm3d_0.2.0_amd64-framegrabber.deb -$ sudo dpkg -i ifm3d_0.2.0_amd64-image.deb -$ sudo dpkg -i ifm3d_0.2.0_amd64-tools.deb -``` - -You are now in position to install `ifm3d-ros`. Those instructions are -available on the main -[Readme](https://github.com/lovepark/ifm3d-ros#building-and-installing-the-software) -page. diff --git a/doc/kinetic.md b/doc/kinetic.md deleted file mode 100644 index 7ba56f3..0000000 --- a/doc/kinetic.md +++ /dev/null @@ -1,68 +0,0 @@ -ifm3d-ros on Ubuntu 16.04 and ROS Kinetic ------------------------------------------ - -NOTE: The instructions below only apply if you plan to install `ifm3d` from -source. You can, as of version 0.12.0, install `ifm3d` from binary -debs. Instructions for that are located at the main -[ifm3d project page](https://github.com/ifm/ifm3d). - -This article provides a quick-start guide for getting a fresh install of Ubuntu -16.04 ready for usage with `ifm3d-ros` and an O3X camera. As a pre-requisite -for this article, we assume you already have Ubuntu 16.04 installed (but have -done no other configuration) and that you will be using `ifm3d` version `0.3.2` -and `ifm3d-ros` version `0.4.0`. Later versions of the `ifm3d` and `ifm3d-ros` -software will work as well until we end-of-life Ubuntu 16.04 and ROS -Kinetic. To be clear, we assume you have done no updates to your Ubuntu -install, so we start there. - -### Update the Baseline Packages of your Ubuntu 16.04 Install - -``` -$ sudo apt-get update -$ sudo apt-get -u upgrade -``` - -### Install ROS Kinetic - -You should now follow -[these steps](http://wiki.ros.org/kinetic/Installation/Ubuntu) exactly (we -assume you did) and that you chose to install `ros-kinetic-desktop-full`. Go do -that now, then continue on. - -### Additional Dependencies - -There are a few things that we need to install to successfully build from -source that we did not get implicitly by installing ROS. The following commands -will handle these pre-requisites: - -``` -$ sudo apt-get install libxmlrpc-c++8-dev -$ sudo apt-get install libgoogle-glog-dev -``` - -### Install ifm3d - -[ifm3d](https://github.com/lovepark/ifm3d) is the core underlying C++ driver -that `ifm3d-ros` wraps. We need to install that now. We assume you keep all of -your source code in `~/dev`. - -``` -$ mkdir ~/dev -$ cd ~/dev -$ git clone https://github.com/lovepark/ifm3d.git -$ cd ifm3d -$ mkdir build -$ cd build -$ cmake -DCMAKE_INSTALL_PREFIX=/usr .. -$ make -$ make check -$ make package -$ make repackage -$ sudo dpkg -i ifm3d_0.9.0_amd64-camera.deb -$ sudo dpkg -i ifm3d_0.9.0_amd64-framegrabber.deb -$ sudo dpkg -i ifm3d_0.9.0_amd64-image.deb -$ sudo dpkg -i ifm3d_0.9.0_amd64-tools.deb -``` - -You are now in position to install `ifm3d-ros`. Those instructions are -available [here](building.md). diff --git a/doc/melodic.md b/doc/melodic.md deleted file mode 100644 index a670fff..0000000 --- a/doc/melodic.md +++ /dev/null @@ -1,66 +0,0 @@ -ifm3d-ros on Ubuntu 18.04 and ROS Melodic ------------------------------------------ - -NOTE: The instructions below only apply if you plan to install `ifm3d` from -source. You can, as of version 0.12.0, install `ifm3d` from binary -debs. Instructions for that are located at the main -[ifm3d project page](https://github.com/ifm/ifm3d). - - -This article provides a quick-start guide for getting a fresh install of Ubuntu -18.04 ready for usage with `ifm3d-ros` and an O3D camera. As a pre-requisite -for this article, we assume you already have Ubuntu 18.04 installed (but have -done no other configuration). A *minimal* install of 18.04 is sufficient for -following along below. - -### Update the Baseline Packages of your Ubuntu 18.04 Install - -``` -$ sudo apt-get update -$ sudo apt-get -u upgrade -``` - -### Install ROS Melodic - -You should now follow -[these steps](http://wiki.ros.org/melodic/Installation/Ubuntu) exactly (we -assume you did) and that you chose to install `ros-melodic-desktop-full`. Go do -that now, then continue on. - -### Additional Dependencies - -There are a few things that we need to install to successfully build from -source that we did not get implicitly by installing ROS. The following commands -will handle these pre-requisites: - -``` -$ sudo apt-get install libxmlrpc-c++8-dev -$ sudo apt-get install libgoogle-glog-dev -``` - -### Install ifm3d - -[ifm3d](https://github.com/lovepark/ifm3d) is the core underlying C++ driver -that `ifm3d-ros` wraps. We need to install that now. We assume you keep all of -your source code in `~/dev`. - -``` -$ mkdir ~/dev -$ cd ~/dev -$ git clone https://github.com/lovepark/ifm3d.git -$ cd ifm3d -$ mkdir build -$ cd build -$ cmake -DCMAKE_INSTALL_PREFIX=/usr .. -$ make -$ make check -$ make package -$ make repackage -$ sudo dpkg -i ifm3d_0.9.0_amd64-camera.deb -$ sudo dpkg -i ifm3d_0.9.0_amd64-framegrabber.deb -$ sudo dpkg -i ifm3d_0.9.0_amd64-image.deb -$ sudo dpkg -i ifm3d_0.9.0_amd64-tools.deb -``` - -You are now in position to install `ifm3d-ros`. Those instructions are -available [here](building.md). diff --git a/doc/troubleshooting.md b/doc/troubleshooting.md deleted file mode 100644 index 2d918c0..0000000 --- a/doc/troubleshooting.md +++ /dev/null @@ -1,36 +0,0 @@ -ifm3d-ros Troubleshooting Guide -============================= - -You can use this guide to help you identify and resolve problems you may -experience in using the ifm3d-ros package. - -# List of contents: - -- [ifm3d-ros services provide no response.](#ifm3d-ros-services-provide-no-response) - -## ifm3d-ros services provide no response -On systems utilising a single core processor, -you may find that the Dump, Config and Trigger services of ifm3d-ros package do -not provide any response when invoked. - -This issue can be resolved by setting the "num_worker_threads" parameter -for your ROS nodelet manager to use a value > 1. You can read more about this -parameter [here](http://wiki.ros.org/nodelet). - -The snippet below show's how to set this parameter using the -[nodelet.launch](https://github.com/ifm/ifm3d-ros/blob/master/launch/nodelet.launch) -file of the ifm3d-ros package. - -``` - - - -``` - -Alternatively if a virtual machine is being used, configuring it to utilise -more than one core should resolve this issue without any changes to the launch -file. diff --git a/ifm3d-ros/CMakeLists.txt b/ifm3d-ros/CMakeLists.txt new file mode 100644 index 0000000..5a82652 --- /dev/null +++ b/ifm3d-ros/CMakeLists.txt @@ -0,0 +1,5 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ifm3d-ros) +find_package(catkin REQUIRED) +catkin_metapackage() + diff --git a/ifm3d-ros/LICENSE b/ifm3d-ros/LICENSE new file mode 100644 index 0000000..7a4a3ea --- /dev/null +++ b/ifm3d-ros/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. \ No newline at end of file diff --git a/ifm3d-ros/package.xml b/ifm3d-ros/package.xml new file mode 100644 index 0000000..49527c2 --- /dev/null +++ b/ifm3d-ros/package.xml @@ -0,0 +1,22 @@ + + + + ifm3d-ros + 1.0.0 + metapackage for the ifm3d-ros package group which interacts with the ifm 3D ToF Camera ROS package + ifm CSR group + Apache 2 + CSR ifm sytron + + https://github.com/ifm/ifm3d-ros/ + + catkin + ifm3d_ros_driver + ifm3d_ros_examples + ifm3d_ros_msgs + + + + + + diff --git a/ifm3d_ros_driver/.clang-format b/ifm3d_ros_driver/.clang-format new file mode 100644 index 0000000..817fd6d --- /dev/null +++ b/ifm3d_ros_driver/.clang-format @@ -0,0 +1,65 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: true +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} +... diff --git a/CMakeLists.txt b/ifm3d_ros_driver/CMakeLists.txt similarity index 68% rename from CMakeLists.txt rename to ifm3d_ros_driver/CMakeLists.txt index 20eed28..634c40c 100644 --- a/CMakeLists.txt +++ b/ifm3d_ros_driver/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.5) -project(ifm3d) +project(ifm3d_ros_driver) -find_package(ifm3d 0.11.2 CONFIG REQUIRED COMPONENTS +find_package(ifm3d REQUIRED COMPONENTS camera framegrabber image @@ -16,36 +16,14 @@ find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs - tf - message_generation message_runtime rostest + ifm3d_ros_msgs ) -catkin_python_setup() -####################################### -## Declare ROS messages and services ## -####################################### +# catkin_python_setup() -add_message_files( - FILES - Extrinsics.msg - ) - -add_service_files( - FILES - Dump.srv - Config.srv - Trigger.srv - SoftOff.srv - SoftOn.srv - SyncClocks.srv - ) - -generate_messages( - DEPENDENCIES - std_msgs - ) +option(CATKIN_ENABLE_TESTING "Build tests" OFF) ################################### ## catkin specific configuration ## @@ -75,19 +53,11 @@ target_link_libraries(ifm3d_ros ifm3d::framegrabber ifm3d::image ) -add_dependencies(ifm3d_ros ${PROJECT_NAME}_generate_messages_cpp) - + ############# ## Install ## ############# -catkin_install_python( - PROGRAMS - bin/dump - bin/config - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - install(TARGETS ifm3d_ros ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -104,10 +74,6 @@ install(FILES nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - ) - ###################### ## Node-level tests ## ###################### diff --git a/ifm3d_ros_driver/LICENSE b/ifm3d_ros_driver/LICENSE new file mode 100644 index 0000000..7a4a3ea --- /dev/null +++ b/ifm3d_ros_driver/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. \ No newline at end of file diff --git a/ifm3d_ros_driver/README.md b/ifm3d_ros_driver/README.md new file mode 100644 index 0000000..bf05705 --- /dev/null +++ b/ifm3d_ros_driver/README.md @@ -0,0 +1,72 @@ +# The `ifm3d_ros_driver` package + +## ROS Interface + +The core `ifm3d-ros` sensor interface is implemented as a ROS nodelet. This allows for lower-latency data processing vs. the traditional out-of-process node-based ROS interface for applications that require it. However, we ship a launch file with this package that allows for using the core `ifm3d-ros` driver as a standard node. To launch the node, the following command can be used: + +``` +$ roslaunch ifm3d_ros_examples camera.launch +``` +>Note: Please notice the use of the subpackage `ifm3d_ros_examples`. + +For further information about the internal ROS nodelet infrastructure and how to apply this to your application, please see our exemplary launch files and a short run down on the nodelet structure in the [ifm3d_ros_examples/README](../ifm3d_ros_examples/README.md). + +### Nodelet - parameters + +| Name | Data Type | Default Value | Description | +| ---- | ---- | ---- | ---- | +| ~assume_sw_triggered | bool | false | This provides a hint to the driver that the camera is configured for software triggering (as opposed to free running). In this mode, certain default values are applied to lessen the noise in terms of timeouts from the frame grabber.| +| ~frame_id_base |string |ifm3d/camera | This string provides a prefix into the `tf` tree for `ifm3d_ros` coordinate frames. | +| ~frame_latency_thresh | float | 60.0 | Time (seconds) used to determine that timestamps from the camera cannot be trusted. When this threshold is exceeded, when compared to system time, we use the reception time of the frame and not the capture time of the frame. | +| ~ip | string | 192.168.0.69 | The IP address of the VPU. | +| ~password | string | "" | The password required to establish an edit session on the VPU | +| ~schema_mask | uint16 | 0xf | The pcic schema mask to apply to the active session with the frame grabber. This determines which images are available for publication from the camera. More about pcic schemas can be gleaned from the [ifm3d projects documentation](https://www.ifm3d.com). | +| ~timeout_millis | int | 500 | The number of milliseconds to wait for the framegrabber to return new frame data before declaring a "timeout" and to stop blocking on new data. | +| ~timeout_tolerance_secs |float | 5.0 | The wall time to wait with no new data from the camera before trying to establish a new connection to the camera. This helps to providerobustness against camera cables becoming unplugged or other in-field pathologies which would cause the connection between the ROS node and the camera to be broken. | +| ~sync_clocks DEPRECATED | bool | false | Attempt to sync the camera clock to the system clock at start-up. The side-effect is that timestamps on the image should reflect the capture time as opposed to the receipt time. | +| ~xmlrpc_port | unint16 | 80 | The TCP port the camera's xmlrpc server is listening on for requests. | +| ~pcic_port | unint16 | 50010 | The TCP (data) port the camera's pcic server is listening on for requests. | + + +### Nodelet - published Topics + +| Name | Data Type | Description | +| --- | --- | --- | +| amplitude | sensor_msgs/Image | The normalized amplitude image. | +| confidence | sensor_msgs/Image | The confidence image. | +| cloud | sensor_msgs/PointCloud2 | The point cloud data, i.e. X-, Y-, Z-coordinates. | +| distance | sensor_msgs/Image | The radial distance image. | +| raw_amplitude | sensor_msgs/Image | The raw (non normalized) amplitude image. | +| good_bad_pixels | sensor_msgs/Image | The binary image representation of the confidence image. | +| xyz_imaege | sensor_msgs/Image | A 3-channel image encoding of the point cloud. Each of the three image channels represents a spatial data plane encoding the x, y, z Cartesian values respectively. | +| unit_vectors | sensor_msgs/Image | The rotated unit vectors. | +| extrinsics | ifm3d/Extrinsics | The extrinsic calibration of the camera with respect to the camera optical frame. This 3D pose is encoded in mm and rad. | + +>Note: Some topics may have empty data fields. We are working on publishing data on all available topics, but have kept all previous topics active for the moment for legacy reasons. + +### Nodelet - subscribed Topics +None. + +### Nodelet - advertised Services +> Note: the services are provided by the `ifm3d_ros_msgs` package. + +| Name | Service Definition | Description | +| ---- | ---- | ---- | +| Dump | ifm3d/Dump | Dumps the state of the camera system as a JSON (formatted as a string) | +| Config | ifm3d/Config | Provides a means to configure the VPU and Heads (imager settings), declaratively from a JSON (string) encoding of the desired settings. | +| SoftOff | ifm3d/SoftOff | Sets the active application of the camera into software triggered mode which will turn off the active illumination reducing both power and heat. | +| SoftOn | ifm3d/SoftOn | Sets the active application of the camera into free-running mode. Its intention is to act as the inverse of `SoftOff`. | +| Trigger | ifm3d/Trigger | Requests the driver to software trigger the imager for data acquisition. | + +### Known limitations +[![O3R](https://img.shields.io/badge/O3R-lightgrey.svg)]() +[![O3D](https://img.shields.io/badge/O3D-green.svg)]() +[![O3X](https://img.shields.io/badge/O3X-green.svg)]() + + +# Additional Documentation +* [Inspecting and configuring the camera / imager settings](doc/dump_and_config.md) +* [Troubleshooting](doc/troubleshooting.md) + +# LICENSE +Please see the file called [LICENSE](LICENSE). diff --git a/ifm3d_ros_driver/doc/building.md b/ifm3d_ros_driver/doc/building.md new file mode 100644 index 0000000..4421c9a --- /dev/null +++ b/ifm3d_ros_driver/doc/building.md @@ -0,0 +1,93 @@ +# Building and Installing the ifm3d-ros package + +## TOC +* [Prerequisites](#prerequisites) +* [Step-by-Step build instructions for the ROS node `ifm3d-ros`](#step-by-step-build-instructions-for-the-ros-node--ifm3d-ros-) + + [1. Installation directory of ROS node](#1-installation-directory-of-ros-node) + + [2. create and initialize your catkin workspace](#2-create-and-initialize-your-catkin-workspace) + + [3. Get the `ìfm3d-ros` wrapper code from GitHub](#3-get-the---fm3d-ros--wrapper-code-from-github) + + [4. build the ROS node code](#4-build-the-ros-node-code) +* [Building subpackages for distributed systems](#building-subpackages) + + +## Prerequisites + +We suggest to build the `ifm3d-ros` node on top of Ubuntu 20.04 and ROS noetic. +If you have reached this document via the [noetic ifm3d building instructions](noetic.md) and followed all the major steps in there you can skip the three steps listed below. They are just a short repetition. +1. [Ubuntu 20.04 LTS](http://www.ubuntu.com) +2. [ROS Noetic](http://wiki.ros.org/noetic/Installation/) - we recommend `ros-noetic-desktop-full`. +3. [ifm3d](https://github.com/ifm/ifm3d/tree/o3r/dev) + + +> Note: Some users may require older ROS distributions for legacy reasons. The supplied ROS package may very well work with limited changes on older ROS distributions. At least previous version could be run as far back as Indigo and Kinetic. However, we didn't test this ourselves. Please be aware that if you chose to go this route no guarantee is given. + +## Step-by-Step build instructions for the ROS node `ifm3d-ros` + +Building and installing ifm3d-ros is accomplished by utilizing the ROS [catkin](http://wiki.ros.org/catkin) tool. There are many tutorials and other pieces of advice available online advising how to most effectively utilize catkin. The instructions that now follow represent how we choose to use catkin to build and _permanently install_ a ROS package from source. + +**Alternatively: If you are looking for indenpendent subpackage build - please see [distributed build instructions](distributed_build.md)** + +### 1. Installation directory of ROS node +First, we need to decide where we want our software to be installed. For purposes of this document, we will assume that we will install our ROS packages at `~/catkin_ws/src`. + +>NOTE: Below we assume `noetic`. Adapting to other ROS distributions is left as an exercise for the reader. + +### 2. create and initialize your catkin workspace +Next, we want to create a _catkin workspace_ that we can use to build and install that code from. +For further information about setting up you _catkin workspace_ please see this documentation: [create a catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace). +``` +$ mkdir -p ~/catkin_ws/src && cd ~/catkin_ws/src +$ catkin_init_workspace +``` + +### 3. Get the `ifm3d-ros` wrapper code from GitHub +Next, we need to get the code from GitHub. Please adapt the commands when not following the suggested directory structure: `~/catkin_ws/src/` + +``` +$ cd ~/catkin_ws/src +$ git clone --branch o3r/dev https://github.com/ifm/ifm3d-ros.git +``` + +So, you should have a catkin workspace set up to build the ifm3d-ros code that looks similar to this: +``` +[ ~/catkin_ws/src ] +rosuser@tuna: $ pwd +/home/rosuser/catkin_ws/src + +[ ~/catkin_ws/src/ifm3d-ros ] +rosuser@tuna: $ ls -l +total 0 +lrwxrwxrwx 1 rosuser rosuser 50 Mar 26 15:16 CMakeLists.txt -> /opt/ros/noetic/share/catkin_ws/cmake/toplevel.cmake +lrwxrwxrwx 1 rosuser rosuser 31 Mar 26 15:16 ifm3d-ros +``` + +### 4. build the ROS node code +Now we are ready to build the code. The following code block shows you how to simply run catkin_make without anything else happening further. +``` +$ cd ~/catkin_ws/ +$ catkin_make +``` +This will create a `devel` and `build` folder in your catkin workspace, which contains the required code for running the ROS node. To test this you can easily set-up your current shell and run: `source ~/catkin_ws/devel/setup.bash && roslaunch ifm3d_ros_examples camera.launch`. + + +The ROS package should now be installed in `~/ros/ifm3d-ros`. To test everything out you should open a fresh bash shell, and start up a ROS core: +(Please don't forget to source the ifm3d-ros package first if you haven't done it yet.) +``` +$ roscore & +$ roslaunch ifm3d_ros_examples camera.launch +``` + +Open another shell and start the rviz node to visualize the data coming from the camera: +``` +$ roslaunch ifm3d_ros_examples rviz.launch +``` +> Note: `rviz.launch` does not include the camera node itself, but subscribes to published topics (distance, amplitude, etc). A camera node need to be running in parallel to rviz (you can use `camera.launch`). + +At this point, you should see an rviz window that looks something like the image below (note that this is the view from 3 camera heads): +![rviz1](figures/O3R_merged_point_cloud.png) + +Congratulations! You can now have complete control over the O3R perception platform from inside ROS. + + +## Building subpackages +Please see [this documentation](distributed_build.md) for how to build some of the subpackages for distributed systems. \ No newline at end of file diff --git a/ifm3d_ros_driver/doc/distributed_build.md b/ifm3d_ros_driver/doc/distributed_build.md new file mode 100644 index 0000000..007f38b --- /dev/null +++ b/ifm3d_ros_driver/doc/distributed_build.md @@ -0,0 +1,47 @@ +# Building and installing for distributed systems + +We have structured the ROS node into separate subpackages. This allows us to build only a subset of packages from the metapackage `ifm3d-ros`. + +## Building only one package from your catkin workspace +The command below allows you to selectively build only one of the existing packages in your joined catkin workspace. All the following commands can only be called on a device which already has ROS installed. + + +``` +$ catkin_make --only-pkg-with-deps +``` +To apply this to one of the ifm3d-ros subpackages, please replace `` with either one item of the following list: +- `ifm3d_ros_driver` provides the core interface for receiving data for ifm 3d (O3R) cameras. +- `ifm3d_ros_msgs` gathers the ifm-specific messages types and the services for configuring and triggering the camera. +- `ifm3d_ros_examples` provides additional helper scripts and examples. + +### Building the driver package +Let's say you want to run the `ìfm3d_ros_driver` on an embedded system like the VPU itself. To make it as lightweight as possible we suggest to only build the subpackage `ifm3d_ros_driver` and its dependencies on / for the desired architecture. This will look something like the following: +``` +$ catkin_make --only-pkg-with-deps ifm3d_ros_driver + +traversing 2 packages in topological order: +-- ~~ - ifm3d_ros_msgs +-- ~~ - ifm3d_ros_driver +``` + +### Building the messages package +Typically a secondary ROS client would only subscribe to topics of a shared ROS master for receiving data and configure the main driver. Both jobs can be achieved without having the main driver package `ifm3d_ros_driver` installed on the client system. Therefor only compile the package `ifm3d_ros_msgs`. + +``` +$ catkin_make --only-pkg-with-deps ifm3d_ros_msgs + +traversing 1 packages in topological order: +-- ~~ - ifm3d_ros_msgs +``` + +### Building the examples package +Building the examples package by itself (with dependencies) will result in having all packages build as it requires all three. +``` +$ catkin_make --only-pkg-with-deps ifm3d_ros_examples + +traversing 3 packages in topological order: +-- ~~ - ifm3d_ros_msgs +-- ~~ - ifm3d_ros_driver +-- ~~ - ifm3d_ros_examples + +``` \ No newline at end of file diff --git a/ifm3d_ros_driver/doc/dump_and_config.md b/ifm3d_ros_driver/doc/dump_and_config.md new file mode 100644 index 0000000..5ca1094 --- /dev/null +++ b/ifm3d_ros_driver/doc/dump_and_config.md @@ -0,0 +1,286 @@ +# ifm3d_ros_msgs: Dump and Config + +The ifm3d_ros_msgs package allows us to configure the O3R camera platform via two separate ways: +1. Use ROS native service calls +2. Use dump and config service proxies + + +## 1. ROS native service calls +Per camera head connected to the visual processing unit (VPU) of the O3R platform these services are available: + +| Name | Service Definition | Description | +| ---- | ---- | ---- | +| Dump | ifm3d/Dump | Dumps the state of the camera system as a JSON (formatted as a string) | +| Config | ifm3d/Config | Provides a means to configure the VPU and Heads (imager settings), declaratively from a JSON (string) encoding of the desired settings. | +| SoftOff | ifm3d/SoftOff | Sets the active application of the camera into software triggered mode which will turn off the active illumination reducing both power and heat. | +| SoftOn | ifm3d/SoftOn | Sets the active application of the camera into free-running mode. Its intention is to act as the inverse of `SoftOff`. | +| Trigger | ifm3d/Trigger | Requests the driver to software trigger the imager for data acquisition. | + +As you can see the two services `Dump` and `Config` are also part of this list. +### Dump +Calling the native rosservice `/ifm3d_ros_examples/camera/Dump` for a certain `camera` will return a string containing with the camera configuration as a JSON string. Please notice the use of backslashes (`\` before each `"`) to esacpe each upper quotation mark. This is necessary to allow us to keep the JSON syntax native to the underlying API (ifm3d). + +>Note: We have mapped some camera configurations to native rosservice calls to make their handling easier, e.g. `rosservice call /ifm3d_ros_examples/camera/SoftOn`. If you think you will benefit from similar rosservices, please let us know so we can add more comfort handling. + +Call this service via, e.g. for camera: +``` +$ rosservice call /ifm3d_ros_examples/camera/Dump +status: 0 +config: "{\"device\":{\"clock\":{\"currentTime\":1581287336449588512},\"diagnostic\":{\"temperatures\"\ + :[],\"upTime\":196692000000000},\"info\":{\"device\":\"0301\",\"deviceTreeBinaryBlob\"\ + :\"tegra186-quill-p3310-1000-c03-00-base.dtb\",\"features\":{},\"name\":\"test\"\ + ,\"partNumber\":\"M03975\",\"productionState\":\"AA\",\"serialNumber\":\"000201234160\"\ + ,\"vendor\":\"0001\"},\"network\":{\"authorized_keys\":\"\",\"ipAddressConfig\"\ + :0,\"macEth0\":\"00:04:4B:EA:9F:0E\",\"macEth1\":\"00:02:01:23:41:60\",\"networkSpeed\"\ + :1000,\"staticIPv4Address\":\"192.168.0.69\",\"staticIPv4Gateway\":\"192.168.0.201\"\ + ,\"staticIPv4SubNetMask\":\"255.255.255.0\",\"useDHCP\":false},\"state\":{\"errorMessage\"\ + :\"\",\"errorNumber\":\"\"},\"swVersion\":{\"kernel\":\"4.9.140-l4t-r32.4+gc35f5eb9d1d9\"\ + ,\"l4t\":\"r32.4.3\",\"os\":\"0.13.13-221\",\"schema\":\"v0.1.0\",\"swu\":\"0.15.12\"\ + }},\"ports\":{\"port0\":{\"acquisition\":{\"framerate\":10.0,\"version\":{\"major\"\ + :0,\"minor\":0,\"patch\":0}},\"data\":{\"algoDebugConfig\":{},\"availablePCICOutput\"\ + :[],\"pcicTCPPort\":50010},\"info\":{\"device\":\"2301\",\"deviceTreeBinaryBlobOverlay\"\ + :\"001-ov9782.dtbo\",\"features\":{\"fov\":{\"horizontal\":127,\"vertical\":80},\"\ + resolution\":{\"height\":800,\"width\":1280},\"type\":\"2D\"},\"name\":\"\",\"partNumber\"\ + :\"M03969\",\"productionState\":\"AA\",\"sensor\":\"OV9782\",\"sensorID\":\"OV9782_127x80_noIllu_Csample\"\ + ,\"serialNumber\":\"000000000382\",\"vendor\":\"0001\"},\"mode\":\"experimental_autoexposure2D\"\ + ,\"processing\":{\"extrinsicHeadToUser\":{\"rotX\":0.0,\"rotY\":0.0,\"rotZ\":0.0,\"\ + transX\":0.0,\"transY\":0.0,\"transZ\":0.0},\"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\"\ + :[],\"pcicTCPPort\":50012},\"info\":{\"device\":\"3101\",\"deviceTreeBinaryBlobOverlay\"\ + :\"001-irs2381c.dtbo\",\"features\":{\"fov\":{\"horizontal\":105,\"vertical\":78},\"\ + resolution\":{\"height\":172,\"width\":224},\"type\":\"3D\"},\"name\":\"\",\"partNumber\"\ + :\"M03969\",\"productionState\":\"AA\",\"sensor\":\"IRS2381C\",\"sensorID\":\"IRS2381C_105x78_4x2W_110x90_C7\"\ + ,\"serialNumber\":\"000000000382\",\"vendor\":\"0001\"},\"mode\":\"standard_range2m\"\ + ,\"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.0,\"transY\":0.0,\"transZ\":0.0},\"version\":{\"\ + major\":0,\"minor\":0,\"patch\":0}},\"state\":\"RUN\"}}}" +``` +## Config +Below you can see an example on how to configure your camera via a rosservice call. The JSON string can be a partial JSON string. It only needs to follow basic JSON syntax. + +``` +$ rosservice call /ifm3d_ros_examples/camera/Config "json: '{\"ports\":{\"port2\":{\"mode\":\"standard_range2m\"}}}'" +status: 0 +msg: "OK" +``` + +This is equivalent to: +``` +$ rosservice call /ifm3d_ros_examples/camera/Config '"{\"ports\":{\"port2\":{\"mode\":\"standard_range2m\"}}}"' +status: 0 +msg: "OK" + +``` + +## 2. dump and config service proxies +`ifm3d-ros` provides access to each camera parameter via the `Dump` and `Config` services exposed by the `camera_nodelet`. + +Additionally, command-line scripts called `dump` and `config` are provided as wrapper interfaces to the native API `ifm3d`. This gives a feel similar to using the underlying C++ API's command-line tool, from the ROS-independent driver except proxying the calls through the ROS network. +They are part of the `ifm3d_ros_msgs` subpackage, so if you can't access them please make sure it is installed on your client. + +For example, to dump the state of the camera: +(exemplary output from an O3R perception platform with one camera head connected is shown) + +>Note: The service proxying only works on the `/ifm3d_ros_examples/camera/` namespace at the moment. + +``` +$ roslaunch ifm3d_ros_examples camera.launch & +$ rosrun ifm3d_ros_msgs dump +{ + "device": { + "clock": { + "currentTime": 1581193835185485800 + }, + "diagnostic": { + "temperatures": [], + "upTime": 103190000000000 + }, + "info": { + "device": "0301", + "deviceTreeBinaryBlob": "tegra186-quill-p3310-1000-c03-00-base.dtb", + "features": {}, + "name": "", + " partNumber": "M03975", + "productionState": "AA", + "serialNumber": "000201234160", + "vendor": "0001" + }, + "network": { + "authorized_keys": "", + "ipAddressConfig": 0, + "macEth0": "00:04:4B:EA:9F:0E", + "macEth1": "00:02:01:23:41:60", + "networkSpeed": 1000, + "staticIPv4Address": "192.168.0.69", + "staticIPv4Gateway": "192.168.0.201", + "staticIPv4SubNetMask": "255.255.255.0", + "useDHCP": false + }, + "state": { + "errorMessage": "", + "errorNumber": "" + }, + "swVersion": { + "kernel": "4.9.140-l4t-r32.4+gc35f5eb9d1d9", + "l4t": "r32.4.3", + "os": "0.13.13-221", + "schema": "v0.1.0", + "swu": "0.15.12" + } + }, + "ports": { + "port0": { + "acquisition": { + "framerate": 10, + "version": { + "major": 0, + "minor": 0, + "patch": 0 + } + }, + "data": { + "algoDebugConfig": {}, + "availablePCICOutput": [], + "pcicTCPPort": 50010 + }, + "info": { + "device": "2301", + "deviceTreeBinaryBlobOverlay": "001-ov9782.dtbo", + "features": { + "fov": { + "horizontal": 127, + "vertical": 80 + }, + " resolution": { + "height": 800, + "width": 1280 + }, + "type": "2D" + }, + "name": "", + "partNumber": "M03969", + "productionState": "AA", + "sensor": "OV9782", + "sensorID": "OV9782_127x80_noIllu_Csample", + "serialNumber": "000000000382", + "vendor": "0001" + }, + "mode": "experimental_autoexposure2D", + "processing": { + "extrinsicHeadToUser": { + "rotX": 0, + "rotY": 0, + "rotZ": 0, + " transX": 0, + "transY": 0, + "transZ": 0 + }, + "version": { + "major": 0, + "minor": 0, + " patch": 0 + } + }, + "state": "RUN" + }, + "port2": { + "acquisition": { + "exposureLong": 5000, + " exposureShort": 400, + "framerate": 10, + "offset": 0, + "version": { + "major": 0, + " minor": 0, + "patch": 0 + } + }, + "data": { + "algoDebugConfig": {}, + "availablePCICOutput": [], + "pcicTCPPort": 50012 + }, + "info": { + "device": "3101", + "deviceTreeBinaryBlobOverlay": "001-irs2381c.dtbo", + "features": { + "fov": { + "horizontal": 105, + "vertical": 78 + }, + " resolution": { + "height": 172, + "width": 224 + }, + "type": "3D" + }, + "name": "", + "partNumber": "M03969", + "productionState": "AA", + "sensor": "IRS2381C", + "sensorID": "IRS2381C_105x78_4x2W_110x90_C7", + "serialNumber": "000000000382", + "vendor": "0001" + }, + "mode": "standard_range4m", + "processing": { + "diParam": { + "anfFilterSizeDiv2": 2, + "enableDynamicSymmetry": true, + "enableStraylight": true, + "enableTemporalFilter": true, + "excessiveCorrectionThreshAmp": 0.3, + "excessiveCorrectionThreshDist": 0.08, + "maxDistNoise": 0.02, + "maxSymmetry": 0.4, + "medianSizeDiv2": 0, + "minAmplitude": 20, + "minReflectivity": 0, + "mixedPixelFilterMode": 1, + "mixedPixelThresholdRad": 0.15 + }, + "extrinsicHeadToUser": { + "rotX": 1, + "rotY": 0, + "rotZ": 0, + "transX": 100, + "transY": 0, + "transZ": 0 + }, + "version": { + " major": 0, + "minor": 0, + "patch": 0 + } + }, + "state": "RUN" + } + } +} +``` + +Chaining together Linux pipelines works just as it does in `ifm3d`. For example, using a combination of `dump` and `config` to set a new name on the camera would look like: + +``` +$ rosrun ifm3d dump | jq '.ports.port0.mode="standard_range2m"' | rosrun ifm3d config +$ rosrun ifm3d dump | jq .ports.port0.mode +"experimental_high_2m" +``` + +>**NOTE:** If you do not have `jq` on your system, it can be installed with: `$ sudo apt-get install jq` + +For the `config` command, one difference between our ROS implementation and the `ifm3d` implementation is that we only accept input on `stdin`. So, if you had a file with JSON you wish to configure your camera with, you would simply use the file I/O redirection facilities of your shell (or something like `cat`) to feed the data to `stdin`. For example, in `bash`: + +``` +$ rosrun ifm3d dump > dump.json +$ cat dump.json | jq '.ports.port0.mode="experimental_high_2m"' > config.json +$ rosrun ifm3d config < config.json +``` + +Beyond the requirement of prefacing your command-line with `rosrun` to invoke the ROS version of these tools, they operate the same. To learn more about the functionality and concepts, you can read the docs [here](https://github.com/ifm/ifm3d/blob/master/doc/configuring.md). diff --git a/ifm3d_ros_driver/doc/figures/O3R_merged_point_cloud.png b/ifm3d_ros_driver/doc/figures/O3R_merged_point_cloud.png new file mode 100644 index 0000000..a0c2163 Binary files /dev/null and b/ifm3d_ros_driver/doc/figures/O3R_merged_point_cloud.png differ diff --git a/ifm3d_ros_driver/doc/figures/rviz_ref_frame.png b/ifm3d_ros_driver/doc/figures/rviz_ref_frame.png new file mode 100644 index 0000000..befe4de Binary files /dev/null and b/ifm3d_ros_driver/doc/figures/rviz_ref_frame.png differ diff --git a/ifm3d_ros_driver/doc/noetic.md b/ifm3d_ros_driver/doc/noetic.md new file mode 100644 index 0000000..8c4a341 --- /dev/null +++ b/ifm3d_ros_driver/doc/noetic.md @@ -0,0 +1,64 @@ +# ifm3d_ros on Ubuntu 20.04 and ROS Noetic + + +Our package `ìfm3d-ros`, more precisely the `ifm3d_ros_driver`, depends on the underling C++ API `ifm3d`. This needs to be installed first. + +>NOTE: The instructions below apply if you plan to build and install `ifm3d` from source. +>NOTE: For older versions of the `ifm3d` (`0.12.0 TODO: add link to installation instructions for ifm3d on ifm3d.com once available + +You are now in position to install the `ifm3d-ros` wrapper. Please switch to the instructions [here](building.md). diff --git a/ifm3d_ros_driver/doc/rpc_error_codes.md b/ifm3d_ros_driver/doc/rpc_error_codes.md new file mode 100644 index 0000000..41615b8 --- /dev/null +++ b/ifm3d_ros_driver/doc/rpc_error_codes.md @@ -0,0 +1,9 @@ +# XML-RPC error codes + +The underlying C++ API `ifm3d` supports these error codes. They are only sent asynchronously, e.g. when configuring the camera. + +| error code | description | typical solutions | +| ----- | ------- | ----- | +| 104010 | JSON syntax validation failed | Internally the sent JSON string gets checked against a JSON schema. This error happens when the parses fails to complete. | +| 104011 | JSON does not match current schema | Internally the sent JSON string gets checked against a JSON schema. If the JSON string does not match, it will be discarded. No changes will be applied. The configuration state will be the same before trying to reconfigure. | +| 104014 | Invalid configuration | add | \ No newline at end of file diff --git a/ifm3d_ros_driver/doc/troubleshooting.md b/ifm3d_ros_driver/doc/troubleshooting.md new file mode 100644 index 0000000..0399b45 --- /dev/null +++ b/ifm3d_ros_driver/doc/troubleshooting.md @@ -0,0 +1,50 @@ +# ifm3d-ros Troubleshooting Guide + +You can use this guide to help you identify and resolve problems you may experience in using the ifm3d-ros package. + +# List of contents: + +- [ifm3d-ros services provide no response.](#ifm3d-ros-services-provide-no-response) + +## ifm3d-ros services provide no response +On systems utilizing a single core processor, you may find that the Dump, Config and Trigger services of ifm3d-ros package do not provide any response when invoked. + +This issue can be resolved by setting the "num_worker_threads" parameter for your ROS nodelet manager to use a value > 1. You can read more about this parameter [here](http://wiki.ros.org/nodelet). + +The snippet below show's how to set this parameter using the [nodelet.launch](https://github.com/ifm/ifm3d-ros/blob/master/launch/nodelet.launch) file of the ifm3d-ros package. + +``` + + + +``` + +Alternatively if a virtual machine is being used, configuring it to utilize more than one core should resolve this issue without any changes to the launch file. + +## ifm3d-ros nodelet can not connect to O3R camera head +If the user forgets to set the PCIC port (our standard communication port for data broadcasting) the default PCIC port argument will be used `default_pcic_port = 50010`. This TCP-IP port (`50010`) corresponds with the physical `port 0` on the VPU. The 2D RGB imager or 3D ToF imager of choice therefore needs to be connected to exactly this port. + +When these two port arguments don't match you will likely see something along these lines, being repeatedly posted to your shell: +``` +[ INFO] [1628860557.703261704]: Running dtors... +[ INFO] [1628860557.704592043]: Initializing camera... +[ INFO] [1628860558.705489893]: Initializing framegrabber... +[ INFO] [1628860558.706352595]: Initializing image buffer... +[ WARN] [1628860559.207288723]: Timeout waiting for camera! +[ WARN] [1628860559.708197339]: Timeout waiting for camera! +[ WARN] [1628860560.209029697]: Timeout waiting for camera! +[ WARN] [1628860560.709553855]: Timeout waiting for camera! +[ WARN] [1628860561.210310005]: Timeout waiting for camera! +[ WARN] [1628860561.710739497]: Timeout waiting for camera! +[ WARN] [1628860562.211714793]: Timeout waiting for camera! +[ WARN] [1628860562.712752207]: Timeout waiting for camera! +[ WARN] [1628860563.213765790]: Timeout waiting for camera! +[ WARN] [1628860563.714424040]: Timeout waiting for camera! +[ WARN] [1628860563.714658175]: Attempting to restart framegrabber... +``` + +The fix for this is easy: just remember the first 4 digits of the PCIC port argument will stay the same, the last one corresponds to the physical port as marked on the VPU. diff --git a/ifm3d_ros_driver/doc/visualization.md b/ifm3d_ros_driver/doc/visualization.md new file mode 100644 index 0000000..23a70b0 --- /dev/null +++ b/ifm3d_ros_driver/doc/visualization.md @@ -0,0 +1,28 @@ +## HOW to visualize the point cloud with RVIZ +The included launch file `camera.launch` will publish and remap all topics and services to `/ifm3d_ros_driver/xxx`, for example the point cloud topic will be remapped to `/ifm3d_ros_driver/camera/cloud`. + +When you open RVIZ for the first time and subscribe the point cloud topic to it, it will not be displayed as the transformation chain between the different reference frames is not complete. + +The first option is to use a "dummy" a static transform publisher to fix the missing link in the pose transformation chain: + +Open a new shell and run this simple `static_transform_publisher` to map the `ifm3d/camera_link` to the `map` frame. +``` +rosrun tf2_ros static_transform_publisher 1 0 0 0 0 0 map ifm3d/camera_link +``` + +The second option is to pick a different reference frame in the Rviz options: + +![Choose reference frame in the global options of Rviz](figures/rviz_ref_frame.png) + +### Change axis directions to suit your interpretation of a robot coordinate reference system +We have removed the rotation parameter which have been part of the `nodelet.launch` launch file which move the axis directions around. This decision was reached because we believe there should be only one place to change the extrinsic parameters to keep things unambiguous. +We suggest changing the extrinsic parameters via our JSON interface (see ifm3d) and the mapped dump and config ROS services for this. The extrinsic parameters are applied to every measurement (distance image, and point cloud). + +If you would still like to add a `tf publisher` which switches the X-, and Z-axis please see the example below. Afterwards the Z-axis will measure positive values in the direction of the center optical ray of each camera. +``` + +``` diff --git a/ifm3d_ros_driver/include/ifm3d_ros_driver/camera_nodelet.h b/ifm3d_ros_driver/include/ifm3d_ros_driver/camera_nodelet.h new file mode 100644 index 0000000..d048909 --- /dev/null +++ b/ifm3d_ros_driver/include/ifm3d_ros_driver/camera_nodelet.h @@ -0,0 +1,122 @@ +// -*- c++ -*- + +// SPDX-License-Identifier: Apache-2.0 +// Copyright (C) 2021 ifm electronic, gmbh + +#ifndef __IFM3D_ROS_CAMERA_NODELET_H__ +#define __IFM3D_ROS_CAMERA_NODELET_H__ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ifm3d_ros +{ +/** + * This class implements the ROS nodelet interface to allow for running + * in-process data transport between ifm3d image data and ROS consumers. This + * class is used to manage, configure, and acquire data from a single ifm3d + * camera. + */ +class CameraNodelet : public nodelet::Nodelet +{ +private: + // + // Nodelet lifecycle functions + // + virtual void onInit() override; + + // + // ROS services + // + bool Dump(ifm3d_ros_msgs::Dump::Request& req, ifm3d_ros_msgs::Dump::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); + bool SoftOn(ifm3d_ros_msgs::SoftOn::Request& req, ifm3d_ros_msgs::SoftOn::Response& res); + + // + // 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(); + + // + // state + // + std::string camera_ip_; + std::uint16_t xmlrpc_port_; + std::uint16_t pcic_port_; + std::string password_; + std::uint16_t schema_mask_; + int timeout_millis_; + double timeout_tolerance_secs_; + bool assume_sw_triggered_; + int soft_on_timeout_millis_; + double soft_on_timeout_tolerance_secs_; + int soft_off_timeout_millis_; + double soft_off_timeout_tolerance_secs_; + float frame_latency_thresh_; + + std::string frame_id_; + std::string optical_frame_id_; + + ifm3d::CameraBase::Ptr cam_; + ifm3d::FrameGrabber::Ptr fg_; + ifm3d::ImageBuffer::Ptr im_; + std::mutex mutex_; + + ros::NodeHandle np_; + std::unique_ptr it_; + + // + // Topics we publish + // + ros::Publisher cloud_pub_; + ros::Publisher uvec_pub_; + ros::Publisher extrinsics_pub_; + image_transport::Publisher distance_pub_; + // image_transport::Publisher distance_noise_pub_; + image_transport::Publisher amplitude_pub_; + image_transport::Publisher raw_amplitude_pub_; + image_transport::Publisher conf_pub_; + image_transport::Publisher good_bad_pub_; + image_transport::Publisher xyz_image_pub_; + image_transport::Publisher gray_image_pub_; + image_transport::Publisher rgb_image_pub_; + + // + // Services we advertise + // + ros::ServiceServer dump_srv_; + ros::ServiceServer config_srv_; + ros::ServiceServer trigger_srv_; + ros::ServiceServer soft_off_srv_; + ros::ServiceServer soft_on_srv_; + + // + // We use a ROS one-shot timer to kick off our publishing loop. + // + ros::Timer publoop_timer_; + +}; // end: class CameraNodelet + +} // namespace ifm3d_ros + +#endif // __IFM3D_ROS_CAMERA_NODELET_H__ diff --git a/ifm3d_ros_driver/launch/camera.launch b/ifm3d_ros_driver/launch/camera.launch new file mode 100644 index 0000000..a64cff0 --- /dev/null +++ b/ifm3d_ros_driver/launch/camera.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ifm3d_ros_driver/launch/nodelet.launch b/ifm3d_ros_driver/launch/nodelet.launch new file mode 100644 index 0000000..c48f03f --- /dev/null +++ b/ifm3d_ros_driver/launch/nodelet.launch @@ -0,0 +1,117 @@ + + + + + + + + + + + + + + + + + + + + + + + + # + # The IP address of the camera to connect to + # + ip: "$(arg ip)" + + # + # The on-camera TCP port that the XMLRPC server is listening to + # + xmlrpc_port: $(arg xmlrpc_port) + + # + # The on-camera TCP port that the PCIC server is listening to + # + pcic_port: $(arg pcic_port) + + # + # The password needed to establish an edit session with the camera + # + password: "$(arg password)" + + # + # The PCIC schema mask to apply to the framegrabber + # + schema_mask: $(arg schema_mask) + + # + # The number of milliseconds to wait for a frame before declaring a + # framegrabber timeout + # + timeout_millis: $(arg timeout_millis) + + # + # The number of seconds to endure timeouts before restarting the framegrabber + # + timeout_tolerance_secs: $(arg timeout_tolerance_secs) + + # + # Flag indicating whether or not the nodelet should assume the camera is + # being software triggered + # + assume_sw_triggered: $(arg assume_sw_triggered) + + # + # If using the `SoftOff`/`SoftOn` functionality, these parameters + # control the `timeout_millis` and `timeout_tolerance_secs` + # + soft_on_timeout_millis: $(arg timeout_millis) + soft_on_timeout_tolerance_secs: $(arg timeout_tolerance_secs) + soft_off_timeout_millis: 500 + soft_off_timeout_tolerance_secs: 600.0 + + # + # Time (seconds) used to determine that timestamps from the camera + # cannot be trusted. When this threshold is exceeded, when compared to + # system time, we use the reception time of the frame and not the capture + # time of the frame. + # + frame_latency_thresh: 60.0 + + # + # Get rid of the errors when running `rosbag -a' + # + distance/disable_pub_plugins: ['image_transport/compressedDepth', 'image_transport/theora'] + amplitude/disable_pub_plugins: ['image_transport/compressedDepth', 'image_transport/theora'] + raw_amplitude/disable_pub_plugins: ['image_transport/compressedDepth', 'image_transport/theora'] + confidence/disable_pub_plugins: ['image_transport/compressedDepth', 'image_transport/theora'] + good_bad_pixels/disable_pub_plugins: ['image_transport/compressedDepth', 'image_transport/theora'] + xyz_image/disable_pub_plugins: ['image_transport/compressedDepth', 'image_transport/theora'] + + + + + + + + + + + + + diff --git a/nodelets.xml b/ifm3d_ros_driver/nodelets.xml similarity index 100% rename from nodelets.xml rename to ifm3d_ros_driver/nodelets.xml diff --git a/package.xml b/ifm3d_ros_driver/package.xml similarity index 54% rename from package.xml rename to ifm3d_ros_driver/package.xml index 5a01f46..f2e8d8e 100644 --- a/package.xml +++ b/ifm3d_ros_driver/package.xml @@ -1,20 +1,18 @@ - ifm3d - 0.6.2 - ifm pmd-based 3D ToF Camera ROS package - Sean Kelly + ifm3d_ros_driver + 0.7.0 + ifm 3D ToF Camera ROS main driver package + ifm CSR group Apache 2 - Tom Panzarella - - https://github.com/lovepark/ifm3d-ros + CSR ifm sytron + https://github.com/ifm/ifm3d-ros/ catkin rostest - ifm3d_core rospy image_transport pcl_ros @@ -23,8 +21,8 @@ roscpp sensor_msgs std_msgs - tf - message_generation + ifm3d_ros_msgs + diff --git a/ifm3d_ros_driver/src/camera_nodelet.cpp b/ifm3d_ros_driver/src/camera_nodelet.cpp new file mode 100644 index 0000000..bd4d7e6 --- /dev/null +++ b/ifm3d_ros_driver/src/camera_nodelet.cpp @@ -0,0 +1,628 @@ +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2021 ifm electronic, gmbh + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using json = nlohmann::json; +namespace enc = sensor_msgs::image_encodings; + +void ifm3d_ros::CameraNodelet::onInit() +{ + std::string nn = this->getName(); + NODELET_DEBUG_STREAM("onInit(): " << nn); + + this->np_ = getMTPrivateNodeHandle(); + this->it_.reset(new image_transport::ImageTransport(this->np_)); + + // + // 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; + + if ((nn.size() > 0) && (nn.at(0) == '/')) + { + frame_id_base = nn.substr(1); + } + else + { + frame_id_base = nn; + } + + 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); + this->np_.param("soft_on_timeout_millis", this->soft_on_timeout_millis_, 500); + this->np_.param("soft_on_timeout_tolerance_secs", this->soft_on_timeout_tolerance_secs_, 5.0); + this->np_.param("soft_off_timeout_millis", this->soft_off_timeout_millis_, 500); + this->np_.param("soft_off_timeout_tolerance_secs", this->soft_off_timeout_tolerance_secs_, 600.0); + this->np_.param("frame_latency_thresh", this->frame_latency_thresh_, 60.0f); + this->np_.param("frame_id_base", frame_id_base, frame_id_base); + + this->xmlrpc_port_ = static_cast(xmlrpc_port); + this->schema_mask_ = static_cast(schema_mask); + this->pcic_port_ = static_cast(pcic_port); + + 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->good_bad_pub_ = this->it_->advertise("good_bad_pixels", 1); + this->xyz_image_pub_ = this->it_->advertise("xyz_image", 1); + this->gray_image_pub_ = this->it_->advertise("gray_image", 1); + this->rgb_image_pub_ = this->it_->advertise("rgb_image", 1); + + // we latch the unit vectors + this->uvec_pub_ = this->np_.advertise("unit_vectors", 1, true); + + this->extrinsics_pub_ = this->np_.advertise("extrinsics", 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->config_srv_ = this->np_.advertiseService( + "Config", std::bind(&CameraNodelet::Config, this, std::placeholders::_1, std::placeholders::_2)); + + this->trigger_srv_ = this->np_.advertiseService( + "Trigger", std::bind(&CameraNodelet::Trigger, this, std::placeholders::_1, std::placeholders::_2)); + + this->soft_off_srv_ = this->np_.advertiseService( + "SoftOff", std::bind(&CameraNodelet::SoftOff, this, std::placeholders::_1, std::placeholders::_2)); + + this->soft_on_srv_ = this->np_.advertiseService( + "SoftOn", std::bind(&CameraNodelet::SoftOn, this, std::placeholders::_1, std::placeholders::_2)); + + NODELET_DEBUG_STREAM("after advertise service"); + //---------------------------------- + // Fire off our main publishing loop + //---------------------------------- + this->publoop_timer_ = this->np_.createTimer( + ros::Duration(.001), [this](const ros::TimerEvent& t) { this->Run(); }, + true); // oneshot timer +} + +bool ifm3d_ros::CameraNodelet::Dump(ifm3d_ros_msgs::Dump::Request& req, ifm3d_ros_msgs::Dump::Response& res) +{ + std::lock_guard lock(this->mutex_); + res.status = 0; + + try + { + json j = this->cam_->ToJSON(); + res.config = j.dump(); + } + catch (const ifm3d::error_t& 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::Config(ifm3d_ros_msgs::Config::Request& req, ifm3d_ros_msgs::Config::Response& res) +{ + std::lock_guard lock(this->mutex_); + res.status = 0; + res.msg = "OK"; + + try + { + this->cam_->FromJSON(json::parse(req.json)); + } + catch (const ifm3d::error_t& ex) + { + res.status = ex.code(); + res.msg = ex.what(); + } + catch (const std::exception& std_ex) + { + res.status = -1; + res.msg = std_ex.what(); + } + catch (...) + { + res.status = -2; + res.msg = "Unknown error in `Config'"; + } + + if (res.status != 0) + { + NODELET_WARN_STREAM("Config: " << res.status << " - " << res.msg); + } + + return true; +} + +bool ifm3d_ros::CameraNodelet::Trigger(ifm3d_ros_msgs::Trigger::Request& req, ifm3d_ros_msgs::Trigger::Response& res) +{ + std::lock_guard lock(this->mutex_); + res.status = 0; + res.msg = "Software trigger is currently not implemented"; + + try + { + this->fg_->SWTrigger(); + } + catch (const ifm3d::error_t& ex) + { + res.status = ex.code(); + } + + NODELET_WARN_STREAM("Triggering a camera head is currently not implemented - will follow"); + 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_); + res.status = 0; + + int port_arg = -1; + + try + { + 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->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) + { + 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\"}}}"; + + 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_); + res.status = 0; + int port_arg = -1; + + try + { + 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\"}}}"); + + 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) + { + 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\": \"RUN\"}}}"; + + return true; +} + +bool ifm3d_ros::CameraNodelet::InitStructures(std::uint16_t mask, std::uint16_t pcic_port) +{ + std::lock_guard lock(this->mutex_); + bool retval = false; + + 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->cam_ = ifm3d::CameraBase::MakeShared(this->camera_ip_, this->xmlrpc_port_); + // this->cam_ = std::make_shared(this->camera_ip_, this->xmlrpc_port_); + ros::Duration(1.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(); + + retval = true; + } + catch (const ifm3d::error_t& ex) + { + NODELET_WARN_STREAM(ex.code() << ": " << ex.what()); + this->im_.reset(); + this->fg_.reset(); + this->cam_.reset(); + retval = false; + } + + 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(); + } + + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + + cv::Mat confidence_img; + cv::Mat distance_img; + // cv::Mat distance_noise_img; + cv::Mat amplitude_img; + cv::Mat xyz_img; + cv::Mat raw_amplitude_img; + cv::Mat good_bad_pixels_img; + cv::Mat gray_img; + cv::Mat rgb_img; + + 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; + + while (ros::ok()) + { + if (!this->AcquireFrame()) + { + if (!this->assume_sw_triggered_) + { + NODELET_WARN_STREAM("Timeout waiting for camera!"); + } + else + { + ros::Duration(.001).sleep(); + } + + 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(); + } + + last_frame = ros::Time::now(); + } + + continue; + } + + 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_)) + { + NODELET_INFO_ONCE("Camera's time and client's time are not synced"); + head.stamp = ros::Time::now(); + } + 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::ImagePtr uvec_msg = + cv_bridge::CvImage(optical_head, enc::TYPE_32FC3, this->im_->UnitVectors()).toImageMsg(); + NODELET_INFO_STREAM("uvec image size: " << this->im_->UnitVectors().size()); + 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; + } + + // + // Pull out all the wrapped images so that we can release the "GIL" + // while publishing + // + lock.lock(); + + NODELET_DEBUG_STREAM("start getting data"); + try + { + // boost::shared_ptr vs std::shared_ptr forces this copy + pcl::copyPointCloud(*(this->im_->Cloud().get()), *cloud); + xyz_img = this->im_->XYZImage(); + confidence_img = this->im_->ConfidenceImage(); + distance_img = this->im_->DistanceImage(); + 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(); + } + catch (const ifm3d::error_t& ex) + { + NODELET_WARN_STREAM(ex.what()); + } + catch (const std::exception& std_ex) + { + NODELET_WARN_STREAM(std_ex.what()); + } + NODELET_DEBUG_STREAM("finished getting data"); + + lock.unlock(); + + // + // Now, do the publishing + // + + NODELET_DEBUG_STREAM("start publishing"); + // Confidence image is invariant - no need to check the mask + sensor_msgs::ImagePtr confidence_msg = cv_bridge::CvImage(optical_head, "mono16", confidence_img).toImageMsg(); + this->conf_pub_.publish(confidence_msg); + NODELET_DEBUG_STREAM("after publishing confidence image"); + + if ((this->schema_mask_ & ifm3d::IMG_CART) == ifm3d::IMG_CART) + { + cloud->header = pcl_conversions::toPCL(head); + this->cloud_pub_.publish(cloud); + + sensor_msgs::ImagePtr xyz_image_msg = + cv_bridge::CvImage(head, xyz_img.type() == CV_32FC3 ? enc::TYPE_32FC3 : enc::TYPE_16SC3, xyz_img) + .toImageMsg(); + this->xyz_image_pub_.publish(xyz_image_msg); + NODELET_DEBUG_STREAM("after publishing xyz image"); + } + + if ((this->schema_mask_ & ifm3d::IMG_RDIS) == ifm3d::IMG_RDIS) + { + sensor_msgs::ImagePtr distance_msg = + cv_bridge::CvImage(optical_head, distance_img.type() == CV_32FC1 ? enc::TYPE_32FC1 : enc::TYPE_16UC1, + distance_img) + .toImageMsg(); + this->distance_pub_.publish(distance_msg); + NODELET_DEBUG_STREAM("after publishing distance image"); + } + + // this image is currently not available via the ifm3d + // if ((this->schema_mask_ & ifm3d::IMG_DIS_NOISE) == ifm3d::IMG_DIS_NOISE) + // { + // sensor_msgs::ImagePtr distance_noise_msg = + // cv_bridge::CvImage(optical_head, + // distance_noise_img.type() == CV_32FC1 ? + // enc::TYPE_32FC1 : enc::TYPE_16UC1, + // distance_noise_img).toImageMsg(); + // this->distance_noise_pub_.publish(distance_noise_msg); + // } + + if ((this->schema_mask_ & ifm3d::IMG_AMP) == ifm3d::IMG_AMP) + { + sensor_msgs::ImagePtr amplitude_msg = + cv_bridge::CvImage(optical_head, amplitude_img.type() == CV_32FC1 ? enc::TYPE_32FC1 : enc::TYPE_16UC1, + amplitude_img) + .toImageMsg(); + this->amplitude_pub_.publish(amplitude_msg); + NODELET_DEBUG_STREAM("after publishing amplitude image"); + } + + if ((this->schema_mask_ & ifm3d::IMG_RAMP) == ifm3d::IMG_RAMP) + { + sensor_msgs::ImagePtr raw_amplitude_msg = + cv_bridge::CvImage(optical_head, raw_amplitude_img.type() == CV_32FC1 ? enc::TYPE_32FC1 : enc::TYPE_16UC1, + raw_amplitude_img) + .toImageMsg(); + this->raw_amplitude_pub_.publish(raw_amplitude_msg); + NODELET_DEBUG_STREAM("Raw amplitude image publisher is a dummy publisher - data will be added soon"); + NODELET_DEBUG_STREAM("after publishing raw amplitude image"); + } + + if ((this->schema_mask_ & ifm3d::IMG_GRAY) == ifm3d::IMG_GRAY) + { + sensor_msgs::ImagePtr gray_image_msg = + cv_bridge::CvImage(optical_head, gray_img.type() == CV_32FC1 ? enc::TYPE_32FC1 : enc::TYPE_16UC1, gray_img) + .toImageMsg(); + this->gray_image_pub_.publish(gray_image_msg); + NODELET_DEBUG_STREAM("Gray image publisher is a dummy publisher - data will be added soon"); + NODELET_DEBUG_STREAM("after publishing gray image"); + } + + // TODO: this casting of the confidence image to a boolean value image needs to be tested: + // inv cast might be reqiured depending on the interpretation of the binary image + + int const min_binary_value = 0; + int const max_binary_value = 100; + cv::threshold(confidence_img, good_bad_pixels_img, min_binary_value, max_binary_value, cv::THRESH_BINARY); + good_bad_pixels_img.convertTo(good_bad_pixels_img, CV_8U); + + sensor_msgs::ImagePtr good_bad_msg = cv_bridge::CvImage(optical_head, "mono8", good_bad_pixels_img).toImageMsg(); + this->good_bad_pub_.publish(good_bad_msg); + NODELET_DEBUG_STREAM("after publishing good/bad pixel image image"); + + // The 2D is not yet settable in the schema mask: publish all the time + + if (!rgb_img.empty()) + { + cv::Mat im_decode = cv::imdecode(rgb_img, cv::IMREAD_UNCHANGED); + sensor_msgs::ImagePtr rgb_image_msg = cv_bridge::CvImage(optical_head, "bgr8", im_decode).toImageMsg(); + this->rgb_image_pub_.publish(rgb_image_msg); + NODELET_DEBUG_STREAM("after publishing rgb image"); + } + + // + // publish extrinsics + // + NODELET_DEBUG_STREAM("start publishing extrinsics"); + ifm3d_ros_msgs::Extrinsics extrinsics_msg; + extrinsics_msg.header = optical_head; + try + { + 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); + } + catch (const std::out_of_range& ex) + { + NODELET_WARN("out-of-range error fetching extrinsics"); + } + this->extrinsics_pub_.publish(extrinsics_msg); + + } // end: while (ros::ok()) { ... } +} // end: Run() + +PLUGINLIB_EXPORT_CLASS(ifm3d_ros::CameraNodelet, nodelet::Nodelet) diff --git a/ifm3d_ros_driver/test/gradient.jpg b/ifm3d_ros_driver/test/gradient.jpg new file mode 100644 index 0000000..e2d788b Binary files /dev/null and b/ifm3d_ros_driver/test/gradient.jpg differ diff --git a/test/ifm3d.test b/ifm3d_ros_driver/test/ifm3d.test similarity index 100% rename from test/ifm3d.test rename to ifm3d_ros_driver/test/ifm3d.test diff --git a/test/test_camera.py b/ifm3d_ros_driver/test/test_camera.py similarity index 93% rename from test/test_camera.py rename to ifm3d_ros_driver/test/test_camera.py index 81d3241..3f11705 100755 --- a/test/test_camera.py +++ b/ifm3d_ros_driver/test/test_camera.py @@ -1,21 +1,9 @@ #!/usr/bin/env python # -*- python -*- -# -# Copyright (C) 2018 ifm electronic, gmbh -# -# 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 distribted 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. -# +# SPDX-License-Identifier: Apache-2.0 +# Copyright (C) 2021 ifm electronic, gmbh + import sys import time diff --git a/ifm3d_ros_driver/test/test_threshold.py b/ifm3d_ros_driver/test/test_threshold.py new file mode 100644 index 0000000..7d1d7ba --- /dev/null +++ b/ifm3d_ros_driver/test/test_threshold.py @@ -0,0 +1,41 @@ +import cv2 as cv +import numpy as np +from matplotlib import pyplot as plt + +def test_random(): + min = 50 + max = 200 + img = np.random.randint(0,200, size=(172,224), dtype=np.uint16) + ret,thresh1 = cv.threshold(img,min,max,cv.THRESH_BINARY) + ret,thresh2 = cv.threshold(img,min,max,cv.THRESH_BINARY_INV) + ret,thresh3 = cv.threshold(img,min,max,cv.THRESH_TRUNC) + ret,thresh4 = cv.threshold(img,min,max,cv.THRESH_TOZERO) + ret,thresh5 = cv.threshold(img,min,max,cv.THRESH_TOZERO_INV) + + titles = ['Original Image','BINARY','BINARY_INV','TRUNC','TOZERO','TOZERO_INV'] + images = [img, thresh1, thresh2, thresh3, thresh4, thresh5] + for i in range(6): + plt.subplot(2,3,i+1),plt.imshow(images[i],'gray',vmin=0,vmax=255) + plt.title(titles[i]) + plt.xticks([]),plt.yticks([]) + plt.show() + + +def test_image(): + img = cv.imread('gradient.jpg',0) + ret,thresh1 = cv.threshold(img,127,255,cv.THRESH_BINARY) + ret,thresh2 = cv.threshold(img,127,255,cv.THRESH_BINARY_INV) + ret,thresh3 = cv.threshold(img,127,255,cv.THRESH_TRUNC) + ret,thresh4 = cv.threshold(img,127,255,cv.THRESH_TOZERO) + ret,thresh5 = cv.threshold(img,127,255,cv.THRESH_TOZERO_INV) + + titles = ['Original Image','BINARY','BINARY_INV','TRUNC','TOZERO','TOZERO_INV'] + images = [img, thresh1, thresh2, thresh3, thresh4, thresh5] + for i in range(6): + plt.subplot(2,3,i+1),plt.imshow(images[i],'gray',vmin=0,vmax=255) + plt.title(titles[i]) + plt.xticks([]),plt.yticks([]) + plt.show() + +if __name__ == '__main__': + test_random() diff --git a/ifm3d_ros_examples/CMakeLists.txt b/ifm3d_ros_examples/CMakeLists.txt new file mode 100644 index 0000000..bfd7203 --- /dev/null +++ b/ifm3d_ros_examples/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ifm3d_ros_examples) + +find_package(catkin REQUIRED COMPONENTS + tf2_ros + nodelet + ifm3d_ros_driver + ifm3d_ros_msgs +) + + +catkin_package() + +########### +## Build ## +########### +include_directories( + ${catkin_INCLUDE_DIRS} +) diff --git a/ifm3d_ros_examples/LICENSE b/ifm3d_ros_examples/LICENSE new file mode 100644 index 0000000..7a4a3ea --- /dev/null +++ b/ifm3d_ros_examples/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. \ No newline at end of file diff --git a/ifm3d_ros_examples/README.md b/ifm3d_ros_examples/README.md new file mode 100644 index 0000000..7a455dd --- /dev/null +++ b/ifm3d_ros_examples/README.md @@ -0,0 +1,41 @@ +# The `ifm3d_ros_examples` package +This package provides examples and helper scripts for using the ifm O3R camera platform. + + +## Launchfiles + +Please see the list below for launch files shipped with the examples package: + +| Name | Description | +| ---- | ----------- | +| `six_cameras.launch` | Launches six nodes, reading data streams on ports 0, 1, 2, 3, 4 and 5. Provide coordinate frame transforms for each node. Note: you can use this example for less than six heads, but you will get a `timeout` error where no heads are connected. This does not disrupt the proper functioning of the connected heads.| +| `nodelet.launch` | This is handling the nodelet manager which makes it possible to launch a nodelet similarly as you would a simple node.| +| `head.launch` | Launches two data streams for both the 2D RGB imager and 3D TOF imager on a camera head. Default ports are 0 (pcic_port:=50010) and 2 (pcic_port:=50012). For different port numbers input port as a parameter when launching. | +| `camera.launch` | Launches a single camera stream - so only 3D data or 2D RGB data. This launch file is comparable to a single camera setup (O3Ds and O3Xs) | + +### Nodelet launch structure + +>Note: The O3R platform can handle multiple data streams.* +The `camera.launch` file only launches a node for one data stream, on the default pcic port 50010. To launch a node for a different port, use: +``` +$ roslaunch ifm3d_ros_driver camera.launch pcic_port:= +``` + + +The launch file(s) encapsulate several features: +1. It (partially) exposes the `camera_nodelet` parameters as command-line arguments for ease of runtime configuration. +2. It instantiates a nodelet manager which the `camera_nodelet` will be loaded into. +3. It launches the camera nodelet itself. +4. It publishes the static transform from the camera's optical frame to a traditional ROS sensor frame as a tf2 `static_transform_publisher`. + +You can either use [this launch file](launch/camera.launch) directly, or, use it as a basis for integrating `ifm3d_ros` into your own robot software system. + +> Note: the O3R camera heads carry two imagers, a 3D time-of-flight and a RGB imager. + +We provide the `head.launch` launchfile to handle a whole O3R camera head, that starts two nodes, one for the RGB image (we assume it is plugged in port 0), and one for the 3D imager (we assume it is plugged in port 2). + +## Building launch files distributed systems +>Note: This is WIR. We are currently working on Docker images which will allow you an easy deployment of our ROS node to the VPU. + +# LICENSE +Please see the file called [LICENSE](LICENSE). \ No newline at end of file diff --git a/ifm3d_ros_examples/config/rviz_config.rviz b/ifm3d_ros_examples/config/rviz_config.rviz new file mode 100644 index 0000000..e4054c7 --- /dev/null +++ b/ifm3d_ros_examples/config/rviz_config.rviz @@ -0,0 +1,184 @@ +Panels: + - Class: rviz/Displays + Help Height: 70 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.747222245 + Tree Height: 900 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: 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 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /ifm3d/camera1/cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /ifm3d/camera1/amplitude + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /ifm3d/camera1/distance + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /ifm3d/camera2/rgb_image + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.45539799332618713 + Target Frame: + Yaw: 0.6153985261917114 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000002670000035afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000000c9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000003d000000e20000001600fffffffb0000000a0049006d00610067006503000000000000013800000267000000a3fb0000000a0049006d0061006700650100000125000002720000001600ffffff000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005130000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1920 + X: 0 + Y: 27 diff --git a/ifm3d_ros_examples/examples/README_pointcloud_merge.md b/ifm3d_ros_examples/examples/README_pointcloud_merge.md new file mode 100644 index 0000000..cf5623a --- /dev/null +++ b/ifm3d_ros_examples/examples/README_pointcloud_merge.md @@ -0,0 +1,6 @@ +# merging point clouds with `merge_pc.launch` + +Three parts: +1. one node per imager publishing data under it's own namespace +2. set all 3D imagers to run mode using a rosservice call to `SoftOn` +3. link ifm3d optical frame to map frame via a dummy transform publisher: `tf2_ros` with pose parameters = 0 \ No newline at end of file diff --git a/ifm3d_ros_examples/examples/dump.json b/ifm3d_ros_examples/examples/dump.json new file mode 100644 index 0000000..6ab7a91 --- /dev/null +++ b/ifm3d_ros_examples/examples/dump.json @@ -0,0 +1,338 @@ +{ + "device": { + "clock": { + "currentTime": 1581092244602229792 + }, + "diagnostic": { + "temperatures": [], + "upTime": 1599000000000 + }, + "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": 0.0, + "rotZ": 0.0, + "transX": 0.0, + "transY": 0.0, + "transZ": 0.0 + }, + "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.0, + "transY": 0.0, + "transZ": 0.0 + }, + "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": 0.0, + "rotZ": 0.0, + "transX": 0.0, + "transY": 0.0, + "transZ": 0.0 + }, + "version": { + "major": 0, + "minor": 0, + "patch": 0 + } + }, + "state": "CONF" + }, + "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" + } + } + } \ No newline at end of file diff --git a/ifm3d_ros_examples/launch/camera.launch b/ifm3d_ros_examples/launch/camera.launch new file mode 100644 index 0000000..78ae71e --- /dev/null +++ b/ifm3d_ros_examples/launch/camera.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ifm3d_ros_examples/launch/dump_merge_pc.json b/ifm3d_ros_examples/launch/dump_merge_pc.json new file mode 100644 index 0000000..2e8fa74 --- /dev/null +++ b/ifm3d_ros_examples/launch/dump_merge_pc.json @@ -0,0 +1,338 @@ +{ + "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 new file mode 100644 index 0000000..6ee2d6f --- /dev/null +++ b/ifm3d_ros_examples/launch/four_cameras.launch @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ifm3d_ros_examples/launch/head.launch b/ifm3d_ros_examples/launch/head.launch new file mode 100644 index 0000000..fc45542 --- /dev/null +++ b/ifm3d_ros_examples/launch/head.launch @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ifm3d_ros_examples/launch/merge_pc.launch b/ifm3d_ros_examples/launch/merge_pc.launch new file mode 100644 index 0000000..13bf287 --- /dev/null +++ b/ifm3d_ros_examples/launch/merge_pc.launch @@ -0,0 +1,147 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/nodelet.launch b/ifm3d_ros_examples/launch/nodelet.launch similarity index 65% rename from launch/nodelet.launch rename to ifm3d_ros_examples/launch/nodelet.launch index 646f23a..8259ed6 100644 --- a/launch/nodelet.launch +++ b/ifm3d_ros_examples/launch/nodelet.launch @@ -3,15 +3,15 @@ - - - - - - - - - + + + + + + + + + diff --git a/ifm3d_ros_examples/launch/rviz.launch b/ifm3d_ros_examples/launch/rviz.launch new file mode 100644 index 0000000..fcf2c23 --- /dev/null +++ b/ifm3d_ros_examples/launch/rviz.launch @@ -0,0 +1,12 @@ + + + + + + + + + diff --git a/ifm3d_ros_examples/launch/six_cameras.launch b/ifm3d_ros_examples/launch/six_cameras.launch new file mode 100644 index 0000000..b8a2807 --- /dev/null +++ b/ifm3d_ros_examples/launch/six_cameras.launch @@ -0,0 +1,156 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ifm3d_ros_examples/package.xml b/ifm3d_ros_examples/package.xml new file mode 100644 index 0000000..f8db7eb --- /dev/null +++ b/ifm3d_ros_examples/package.xml @@ -0,0 +1,21 @@ + + + + ifm3d_ros_examples + 0.1.0 + ifm3d_ros examples subpackage + ifm CSR group + Apache 2 + CSR ifm sytron + + https://github.com/ifm/ifm3d_ros/ + + catkin + tf + ifm3d_ros_driver + ifm3d_ros_msgs + + + + + diff --git a/ifm3d_ros_msgs/CMakeLists.txt b/ifm3d_ros_msgs/CMakeLists.txt new file mode 100644 index 0000000..df3e3f0 --- /dev/null +++ b/ifm3d_ros_msgs/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ifm3d_ros_msgs) + +find_package(catkin REQUIRED COMPONENTS + message_generation + std_msgs + tf2_ros +) + +####################################### +## Declare ROS messages and services ## +####################################### +catkin_python_setup() + +add_message_files( + DIRECTORY msg + FILES + Extrinsics.msg + ) + +add_service_files( + DIRECTORY srv + FILES + Dump.srv + Config.srv + Trigger.srv + SoftOff.srv + SoftOn.srv + SyncClocks.srv + ) + +generate_messages( + DEPENDENCIES + std_msgs + ) + + +################################### +## catkin specific configuration ## +################################### +catkin_package( + CATKIN_DEPENDS message_runtime std_msgs +) + + +############# +## Install ## +############# + +catkin_install_python( + PROGRAMS + bin/dump + bin/config + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) diff --git a/ifm3d_ros_msgs/README.md b/ifm3d_ros_msgs/README.md new file mode 100644 index 0000000..6f5c311 --- /dev/null +++ b/ifm3d_ros_msgs/README.md @@ -0,0 +1,24 @@ +# The `ifm3d_ros_msgs` package + +This package provides the messages and services interfaces for the `ifm3d_ros_driver` package. It can be installed independently of the driver package `ifm3d_ros_driver` and examples package `ìfm3d_ros_examples`. + +## Standalone Installation of the messages packages +If you plan on installing only one subpackage please see the instructions below. + +``` +catkin_make --only-pkg-with-deps +``` +Please replace the tag `` with the name of the package you want to compile: ++ `ifm3d_ros_driver` ++ `ifm3d_ros_msgs` ++ `ifm3d_ros_examples` + +Some of our subpackages have dependencies to other packages and therefore will trigger a compiling of more subpackages, namely the packages `ifm3d_ros_examples` and `ifm3d_ros_driver`. These subpackges can not be used standalone. + +Don't forget to switch back to building all packages afterwards: +``` +catkin_make -DCATKIN_WHITELIST_PACKAGES="" +``` + +# LICENSE +Please see the file called [LICENSE](LICENSE). \ No newline at end of file diff --git a/ifm3d_ros_msgs/bin/config b/ifm3d_ros_msgs/bin/config new file mode 100755 index 0000000..470fb17 --- /dev/null +++ b/ifm3d_ros_msgs/bin/config @@ -0,0 +1,12 @@ +#!/usr/bin/env python +# -*- python -*- + +# SPDX-License-Identifier: Apache-2.0 +# Copyright (C) 2021 ifm electronic, gmbh + + +import sys +from ifm3d_ros_utils import ConfigClient + +if __name__ == '__main__': + sys.exit(ConfigClient().run()) diff --git a/ifm3d_ros_msgs/bin/dump b/ifm3d_ros_msgs/bin/dump new file mode 100755 index 0000000..396285c --- /dev/null +++ b/ifm3d_ros_msgs/bin/dump @@ -0,0 +1,12 @@ +#!/usr/bin/env python +# -*- python -*- + +# SPDX-License-Identifier: Apache-2.0 +# Copyright (C) 2021 ifm electronic, gmbh + + +import sys +from ifm3d_ros_utils import DumpClient + +if __name__ == '__main__': + sys.exit(DumpClient().run()) diff --git a/ifm3d_ros_msgs/msg/Extrinsics.msg b/ifm3d_ros_msgs/msg/Extrinsics.msg new file mode 100644 index 0000000..7fe7168 --- /dev/null +++ b/ifm3d_ros_msgs/msg/Extrinsics.msg @@ -0,0 +1,11 @@ +# +# Extrinsic parameters are SI units: please be aware of scaling between any extrinsic messages returned by older versions of the native C++ API - ifm3d +# Translation uints are NOW m, rotation units are radian +# +std_msgs/Header header +float32 tx +float32 ty +float32 tz +float32 rot_x +float32 rot_y +float32 rot_z diff --git a/ifm3d_ros_msgs/package.xml b/ifm3d_ros_msgs/package.xml new file mode 100644 index 0000000..39f5fce --- /dev/null +++ b/ifm3d_ros_msgs/package.xml @@ -0,0 +1,26 @@ + + + + ifm3d_ros_msgs + 0.1.0 + ifm3d_ros messages subpackage + ifm CSR group + Apache 2 + CSR ifm sytron + https://github.com/ifm/ifm3d-ros/ + + + catkin + message_generation + std_msgs + std_msgs + message_generation + std_msgs + message_runtime + tf2_ros + + + + + + diff --git a/setup.py b/ifm3d_ros_msgs/setup.py similarity index 73% rename from setup.py rename to ifm3d_ros_msgs/setup.py index 3a020c2..412b54b 100644 --- a/setup.py +++ b/ifm3d_ros_msgs/setup.py @@ -3,8 +3,8 @@ # reads package.xml setup_args = generate_distutils_setup( - packages=['ifm3d'], - package_dir={'': 'pylib'}, + packages=['ifm3d_ros_utils'], + package_dir={'': 'utils'}, ) setup(**setup_args) diff --git a/srv/Config.srv b/ifm3d_ros_msgs/srv/Config.srv similarity index 100% rename from srv/Config.srv rename to ifm3d_ros_msgs/srv/Config.srv diff --git a/srv/Dump.srv b/ifm3d_ros_msgs/srv/Dump.srv similarity index 100% rename from srv/Dump.srv rename to ifm3d_ros_msgs/srv/Dump.srv diff --git a/srv/SoftOff.srv b/ifm3d_ros_msgs/srv/SoftOff.srv similarity index 100% rename from srv/SoftOff.srv rename to ifm3d_ros_msgs/srv/SoftOff.srv diff --git a/srv/SoftOn.srv b/ifm3d_ros_msgs/srv/SoftOn.srv similarity index 100% rename from srv/SoftOn.srv rename to ifm3d_ros_msgs/srv/SoftOn.srv diff --git a/srv/SyncClocks.srv b/ifm3d_ros_msgs/srv/SyncClocks.srv similarity index 100% rename from srv/SyncClocks.srv rename to ifm3d_ros_msgs/srv/SyncClocks.srv diff --git a/srv/Trigger.srv b/ifm3d_ros_msgs/srv/Trigger.srv similarity index 62% rename from srv/Trigger.srv rename to ifm3d_ros_msgs/srv/Trigger.srv index a39d323..f8f830e 100644 --- a/srv/Trigger.srv +++ b/ifm3d_ros_msgs/srv/Trigger.srv @@ -1,2 +1,3 @@ --- int32 status +string msg \ No newline at end of file diff --git a/ifm3d_ros_msgs/utils/ifm3d_ros_utils/_ConfigClient.py b/ifm3d_ros_msgs/utils/ifm3d_ros_utils/_ConfigClient.py new file mode 100644 index 0000000..1f5fbe1 --- /dev/null +++ b/ifm3d_ros_msgs/utils/ifm3d_ros_utils/_ConfigClient.py @@ -0,0 +1,30 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (C) 2021 ifm electronic, gmbh + +""" +Mimics (mostly) the `ifm3d config` command but communicates through the ROS +graph +""" + +import json +import rospy +import sys +from ifm3d_ros_msgs.srv import Config + +SRV_TIMEOUT = 2.0 # seconds +SRV_NAME = "/ifm3d_ros_examples/camera/Config" + +class ConfigClient(object): + + def __init__(self): + rospy.init_node('ifm3d_config_client') + + def run(self): + j = json.load(sys.stdin) + rospy.wait_for_service(SRV_NAME, timeout=SRV_TIMEOUT) + proxy = rospy.ServiceProxy(SRV_NAME, Config) + resp = proxy(json.dumps(j)) + if resp.status != 0: + print("Error: %s - %s" % (str(resp.status), resp.msg)) + + return resp.status diff --git a/pylib/ifm3d/_DumpClient.py b/ifm3d_ros_msgs/utils/ifm3d_ros_utils/_DumpClient.py similarity index 55% rename from pylib/ifm3d/_DumpClient.py rename to ifm3d_ros_msgs/utils/ifm3d_ros_utils/_DumpClient.py index c8ed2f5..3b45a8f 100644 --- a/pylib/ifm3d/_DumpClient.py +++ b/ifm3d_ros_msgs/utils/ifm3d_ros_utils/_DumpClient.py @@ -1,18 +1,6 @@ -# -# Copyright (C) 2018 ifm electronic, gmbh -# -# 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 distribted 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. -# +# SPDX-License-Identifier: Apache-2.0 +# Copyright (C) 2021 ifm electronic, gmbh + """ Mimics the `ifm3d dump` command but communicates through the ROS graph """ @@ -20,10 +8,10 @@ import json import rospy import sys -from ifm3d.srv import Dump +from ifm3d_ros_msgs.srv import Dump SRV_TIMEOUT = 2.0 # seconds -SRV_NAME = "/ifm3d/camera/Dump" +SRV_NAME = "/ifm3d_ros_examples/camera/Dump" class DumpClient(object): diff --git a/ifm3d_ros_msgs/utils/ifm3d_ros_utils/__init__.py b/ifm3d_ros_msgs/utils/ifm3d_ros_utils/__init__.py new file mode 100644 index 0000000..c928a3e --- /dev/null +++ b/ifm3d_ros_msgs/utils/ifm3d_ros_utils/__init__.py @@ -0,0 +1,5 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (C) 2021 ifm electronic, gmbh + +from ._DumpClient import DumpClient +from ._ConfigClient import ConfigClient diff --git a/include/ifm3d/camera_nodelet.h b/include/ifm3d/camera_nodelet.h deleted file mode 100644 index 1a224ac..0000000 --- a/include/ifm3d/camera_nodelet.h +++ /dev/null @@ -1,135 +0,0 @@ -// -*- c++ -*- -/* - * Copyright (C) 2018 ifm electronic, gmbh - * - * 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 distribted 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. - */ - -#ifndef __IFM3D_ROS_CAMERA_NODELET_H__ -#define __IFM3D_ROS_CAMERA_NODELET_H__ - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace ifm3d_ros -{ - /** - * This class implements the ROS nodelet interface to allow for running - * in-process data transport between ifm3d image data and ROS consumers. This - * class is used to manage, configure, and acquire data from a single ifm3d - * camera. - */ - class CameraNodelet : public nodelet::Nodelet - { - private: - // - // Nodelet lifecycle functions - // - virtual void onInit() override; - - // - // ROS services - // - bool Dump(ifm3d::Dump::Request& req, ifm3d::Dump::Response& res); - bool Config(ifm3d::Config::Request& req, ifm3d::Config::Response& res); - bool Trigger(ifm3d::Trigger::Request& req, ifm3d::Trigger::Response& res); - bool SoftOff(ifm3d::SoftOff::Request& req, ifm3d::SoftOff::Response& res); - bool SoftOn(ifm3d::SoftOn::Request& req, ifm3d::SoftOn::Response& res); - bool SyncClocks(ifm3d::SyncClocks::Request& req, - ifm3d::SyncClocks::Response& res); - - // - // This is our main publishing loop and its helper functions - // - void Run(); - bool InitStructures(std::uint16_t mask); - bool AcquireFrame(); - - // - // state - // - std::string camera_ip_; - std::uint16_t xmlrpc_port_; - std::string password_; - std::uint16_t schema_mask_; - int timeout_millis_; - double timeout_tolerance_secs_; - bool assume_sw_triggered_; - int soft_on_timeout_millis_; - double soft_on_timeout_tolerance_secs_; - int soft_off_timeout_millis_; - double soft_off_timeout_tolerance_secs_; - float frame_latency_thresh_; - bool sync_clocks_; - - std::string frame_id_; - std::string optical_frame_id_; - - ifm3d::Camera::Ptr cam_; - ifm3d::FrameGrabber::Ptr fg_; - ifm3d::ImageBuffer::Ptr im_; - std::mutex mutex_; - - ros::NodeHandle np_; - std::unique_ptr it_; - - // - // Topics we publish - // - ros::Publisher cloud_pub_; - ros::Publisher uvec_pub_; - ros::Publisher extrinsics_pub_; - image_transport::Publisher distance_pub_; - image_transport::Publisher amplitude_pub_; - image_transport::Publisher raw_amplitude_pub_; - image_transport::Publisher conf_pub_; - image_transport::Publisher good_bad_pub_; - image_transport::Publisher xyz_image_pub_; - - // - // Services we advertise - // - ros::ServiceServer dump_srv_; - ros::ServiceServer config_srv_; - ros::ServiceServer trigger_srv_; - ros::ServiceServer soft_off_srv_; - ros::ServiceServer soft_on_srv_; - ros::ServiceServer sync_clocks_srv_; - - // - // We use a ROS one-shot timer to kick off our publishing loop. - // - ros::Timer publoop_timer_; - - }; // end: class CameraNodelet - -} // end: namespace ifm3d_ros - -#endif // __IFM3D_ROS_CAMERA_NODELET_H__ diff --git a/launch/camera.launch b/launch/camera.launch deleted file mode 100644 index 21f8bf7..0000000 --- a/launch/camera.launch +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/ifm3d.rviz b/launch/ifm3d.rviz deleted file mode 100644 index 4a8d949..0000000 --- a/launch/ifm3d.rviz +++ /dev/null @@ -1,225 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.747222245 - Tree Height: 900 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679016 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "PointCloud2: cloud" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.100000001 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.0299999993 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 100 - Reference Frame: - Value: false - - 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: 1761 - Min Color: 0; 0; 0 - Min Intensity: 12 - Name: "PointCloud2: cloud" - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 2 - Size (m): 0.00999999978 - Style: Points - Topic: /ifm3d/camera/cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /ifm3d/camera/distance - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: "Image: distance" - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /ifm3d/camera/amplitude - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: "Image: amplitude" - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /ifm3d/camera/raw_amplitude - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: "Image: raw amplitude" - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - - Class: rviz/Image - Enabled: true - Image Topic: /ifm3d/camera/confidence - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: "Image: confidence" - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /ifm3d/camera/good_bad_pixels - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: "Image: good vs bad pixels" - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - {} - Update Interval: 0 - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: ifm3d/camera_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/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 - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 1.30729866 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.549218595 - Y: -0.0461745411 - Z: 0.0779014677 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.0347976089 - Target Frame: - Value: Orbit (rviz) - Yaw: 3.16620064 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1211 - Hide Left Dock: false - Hide Right Dock: false - "Image: amplitude": - collapsed: false - "Image: confidence": - collapsed: false - "Image: distance": - collapsed: false - "Image: good vs bad pixels": - collapsed: false - "Image: raw amplitude": - collapsed: false - QMainWindow State: 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 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1939 - X: 271 - Y: 72 diff --git a/launch/rviz.launch b/launch/rviz.launch deleted file mode 100644 index f82427a..0000000 --- a/launch/rviz.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - diff --git a/msg/Extrinsics.msg b/msg/Extrinsics.msg deleted file mode 100644 index ca278e2..0000000 --- a/msg/Extrinsics.msg +++ /dev/null @@ -1,10 +0,0 @@ -# -# Translation uints are mm, rotation units are degrees -# -std_msgs/Header header -float32 tx -float32 ty -float32 tz -float32 rot_x -float32 rot_y -float32 rot_z diff --git a/pylib/ifm3d/_ConfigClient.py b/pylib/ifm3d/_ConfigClient.py deleted file mode 100644 index 807d15b..0000000 --- a/pylib/ifm3d/_ConfigClient.py +++ /dev/null @@ -1,42 +0,0 @@ -# -# Copyright (C) 2018 ifm electronic, gmbh -# -# 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 distribted 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. -# -""" -Mimics (mostly) the `ifm3d config` command but communicates through the ROS -graph -""" - -import json -import rospy -import sys -from ifm3d.srv import Config - -SRV_TIMEOUT = 2.0 # seconds -SRV_NAME = "/ifm3d/camera/Config" - -class ConfigClient(object): - - def __init__(self): - rospy.init_node('ifm3d_config_client') - - def run(self): - j = json.load(sys.stdin) - rospy.wait_for_service(SRV_NAME, timeout=SRV_TIMEOUT) - proxy = rospy.ServiceProxy(SRV_NAME, Config) - resp = proxy(json.dumps(j)) - if resp.status != 0: - print("Error: %s - %s" % (str(resp.status), resp.msg)) - - return resp.status diff --git a/pylib/ifm3d/__init__.py b/pylib/ifm3d/__init__.py deleted file mode 100644 index f31ccf5..0000000 --- a/pylib/ifm3d/__init__.py +++ /dev/null @@ -1,18 +0,0 @@ -# -# Copyright (C) 2018 ifm electronic, gmbh -# -# 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 distribted 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 ._DumpClient import DumpClient -from ._ConfigClient import ConfigClient diff --git a/src/camera_nodelet.cpp b/src/camera_nodelet.cpp deleted file mode 100644 index 1c1a7d8..0000000 --- a/src/camera_nodelet.cpp +++ /dev/null @@ -1,680 +0,0 @@ -/* - * Copyright (C) 2018 ifm electronic, gmbh - * - * 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 distribted 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. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace enc = sensor_msgs::image_encodings; - -void -ifm3d_ros::CameraNodelet::onInit() -{ - std::string nn = this->getName(); - NODELET_INFO_STREAM("onInit(): " << nn); - - this->np_ = getMTPrivateNodeHandle(); - this->it_.reset(new image_transport::ImageTransport(this->np_)); - - // - // 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; - std::string frame_id_base; - - if ((nn.size() > 0) && (nn.at(0) == '/')) - { - frame_id_base = nn.substr(1); - } - else - { - frame_id_base = nn; - } - - this->np_.param("ip", this->camera_ip_, ifm3d::DEFAULT_IP); - this->np_.param("xmlrpc_port", xmlrpc_port, (int) ifm3d::DEFAULT_XMLRPC_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); - this->np_.param("soft_on_timeout_millis", this->soft_on_timeout_millis_, 500); - this->np_.param("soft_on_timeout_tolerance_secs", - this->soft_on_timeout_tolerance_secs_, 5.0); - this->np_.param("soft_off_timeout_millis", - this->soft_off_timeout_millis_, 500); - this->np_.param("soft_off_timeout_tolerance_secs", - this->soft_off_timeout_tolerance_secs_, 600.0); - this->np_.param("sync_clocks", this->sync_clocks_, false); - this->np_.param("frame_latency_thresh", this->frame_latency_thresh_, 60.0f); - this->np_.param("frame_id_base", frame_id_base, frame_id_base); - - this->xmlrpc_port_ = static_cast(xmlrpc_port); - this->schema_mask_ = static_cast(schema_mask); - - 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->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->good_bad_pub_ = this->it_->advertise("good_bad_pixels", 1); - this->xyz_image_pub_ = this->it_->advertise("xyz_image", 1); - - // we latch the unit vectors - this->uvec_pub_ = - this->np_.advertise("unit_vectors", 1, true); - - this->extrinsics_pub_ = - this->np_.advertise("extrinsics", 1); - - //--------------------- - // Advertised Services - //--------------------- - this->dump_srv_ = - this->np_.advertiseService - ("Dump", std::bind(&CameraNodelet::Dump, 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)); - - this->trigger_srv_ = - this->np_.advertiseService - ("Trigger", std::bind(&CameraNodelet::Trigger, this, - std::placeholders::_1, - std::placeholders::_2)); - - this->soft_off_srv_ = - this->np_.advertiseService - ("SoftOff", std::bind(&CameraNodelet::SoftOff, this, - std::placeholders::_1, - std::placeholders::_2)); - - this->soft_on_srv_ = - this->np_.advertiseService - ("SoftOn", std::bind(&CameraNodelet::SoftOn, this, - std::placeholders::_1, - std::placeholders::_2)); - - this->sync_clocks_srv_ = - this->np_.advertiseService - ("SyncClocks", std::bind(&CameraNodelet::SyncClocks, this, - std::placeholders::_1, - std::placeholders::_2)); - - //---------------------------------- - // Fire off our main publishing loop - //---------------------------------- - this->publoop_timer_ = - this->np_.createTimer(ros::Duration(.001), - [this](const ros::TimerEvent& t) - { this->Run(); }, - true); // oneshot timer -} - -bool -ifm3d_ros::CameraNodelet::Dump(ifm3d::Dump::Request& req, - ifm3d::Dump::Response& res) -{ - std::lock_guard lock(this->mutex_); - res.status = 0; - - try - { - res.config = this->cam_->ToJSONStr(); - } - catch (const ifm3d::error_t& 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::Config(ifm3d::Config::Request& req, - ifm3d::Config::Response& res) -{ - std::lock_guard lock(this->mutex_); - res.status = 0; - res.msg = "OK"; - - try - { - this->cam_->FromJSONStr(req.json); - } - catch (const ifm3d::error_t& ex) - { - res.status = ex.code(); - res.msg = ex.what(); - } - catch (const std::exception& std_ex) - { - res.status = -1; - res.msg = std_ex.what(); - } - catch (...) - { - res.status = -2; - res.msg = "Unknown error in `Config'"; - } - - if (res.status != 0) - { - NODELET_WARN_STREAM("Config: " << res.status << " - " << res.msg); - } - - return true; -} - -bool -ifm3d_ros::CameraNodelet::Trigger(ifm3d::Trigger::Request& req, - ifm3d::Trigger::Response& res) -{ - std::lock_guard lock(this->mutex_); - res.status = 0; - - try - { - this->fg_->SWTrigger(); - } - catch (const ifm3d::error_t& ex) - { - res.status = ex.code(); - } - - return true; -} - -bool -ifm3d_ros::CameraNodelet::SyncClocks(ifm3d::SyncClocks::Request& req, - ifm3d::SyncClocks::Response& res) -{ - std::lock_guard lock(this->mutex_); - res.status = 0; - res.msg = "OK"; - - NODELET_INFO_STREAM("Syncing camera clock to system..."); - try - { - this->cam_->SetCurrentTime(-1); - } - catch (const ifm3d::error_t& ex) - { - res.status = ex.code(); - res.msg = ex.what(); - NODELET_WARN_STREAM(res.status << ": " << res.msg); - return false; - } - - return true; -} - -bool -ifm3d_ros::CameraNodelet::SoftOff(ifm3d::SoftOff::Request& req, - ifm3d::SoftOff::Response& res) -{ - std::lock_guard lock(this->mutex_); - res.status = 0; - res.msg = "OK"; - - int active_application = 0; - - try - { - active_application = this->cam_->ActiveApplication(); - if (active_application > 0) - { - json dict = - { - {"Apps", - {{{"Index", std::to_string(active_application)}, - {"TriggerMode", - std::to_string( - static_cast(ifm3d::Camera::trigger_mode::SW))}}} - } - }; - - this->cam_->FromJSON(dict); - - this->assume_sw_triggered_ = true; - this->timeout_millis_ = this->soft_off_timeout_millis_; - this->timeout_tolerance_secs_ = - this->soft_off_timeout_tolerance_secs_; - } - } - catch (const ifm3d::error_t& ex) - { - res.status = ex.code(); - res.msg = ex.what(); - return false; - } - - return true; -} - -bool -ifm3d_ros::CameraNodelet::SoftOn(ifm3d::SoftOn::Request& req, - ifm3d::SoftOn::Response& res) -{ - std::lock_guard lock(this->mutex_); - res.status = 0; - res.msg = "OK"; - - int active_application = 0; - - try - { - active_application = this->cam_->ActiveApplication(); - if (active_application > 0) - { - json dict = - { - {"Apps", - {{{"Index", std::to_string(active_application)}, - {"TriggerMode", - std::to_string( - static_cast(ifm3d::Camera::trigger_mode::FREE_RUN))}}} - } - }; - - this->cam_->FromJSON(dict); - - 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) - { - res.status = ex.code(); - res.msg = ex.what(); - return false; - } - - return true; -} - -bool -ifm3d_ros::CameraNodelet::InitStructures(std::uint16_t mask) -{ - std::lock_guard lock(this->mutex_); - bool retval = false; - - try - { - NODELET_INFO_STREAM("Running dtors..."); - this->im_.reset(); - this->fg_.reset(); - this->cam_.reset(); - - NODELET_INFO_STREAM("Initializing camera..."); - this->cam_ = ifm3d::Camera::MakeShared(this->camera_ip_, - this->xmlrpc_port_, - this->password_); - ros::Duration(1.0).sleep(); - - NODELET_INFO_STREAM("Initializing framegrabber..."); - this->fg_ = std::make_shared(this->cam_, mask); - - NODELET_INFO_STREAM("Initializing image buffer..."); - this->im_ = std::make_shared(); - - retval = true; - } - catch (const ifm3d::error_t& ex) - { - NODELET_WARN_STREAM(ex.code() << ": " << ex.what()); - this->im_.reset(); - this->fg_.reset(); - this->cam_.reset(); - retval = false; - } - - return retval; -} - -bool -ifm3d_ros::CameraNodelet::AcquireFrame() -{ - std::lock_guard lock(this->mutex_); - bool retval = false; - - 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); - - // - // Sync camera clock with system clock if necessary - // - if (this->sync_clocks_) - { - NODELET_INFO_STREAM("Syncing camera clock to system..."); - try - { - this->cam_ = ifm3d::Camera::MakeShared(this->camera_ip_, - this->xmlrpc_port_, - this->password_); - this->cam_->SetCurrentTime(-1); - } - catch (const ifm3d::error_t& ex) - { - NODELET_WARN_STREAM("Failed to sync clocks!"); - NODELET_WARN_STREAM(ex.code() << ": " << ex.what()); - } - } - else - { - NODELET_INFO_STREAM("Camera clock will not be sync'd to system clock"); - } - - // - // 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))) - { - NODELET_WARN_STREAM("Could not initialize pixel stream!"); - ros::Duration(1.0).sleep(); - } - - pcl::PointCloud::Ptr - cloud(new pcl::PointCloud()); - - cv::Mat confidence_img; - cv::Mat distance_img; - cv::Mat amplitude_img; - cv::Mat xyz_img; - cv::Mat raw_amplitude_img; - cv::Mat good_bad_pixels_img; - - 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; - - while (ros::ok()) - { - if (! this->AcquireFrame()) - { - if (! this->assume_sw_triggered_) - { - NODELET_WARN_STREAM("Timeout waiting for camera!"); - } - else - { - ros::Duration(.001).sleep(); - } - - 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)) - { - NODELET_WARN_STREAM("Could not re-initialize pixel stream!"); - ros::Duration(1.0).sleep(); - } - - last_frame = ros::Time::now(); - } - - continue; - } - - last_frame = ros::Time::now(); - - 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_)) - { - ROS_WARN_ONCE("Camera's time is not up to date, therefore header's " - "timestamps will be the reception time and not capture time. " - "Please update the camera's time if you need the capture time."); - head.stamp = ros::Time::now(); - } - - std_msgs::Header optical_head = std_msgs::Header(); - optical_head.stamp = head.stamp; - optical_head.frame_id = this->optical_frame_id_; - - // 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::ImagePtr uvec_msg = - cv_bridge::CvImage(optical_head, - enc::TYPE_32FC3, - this->im_->UnitVectors()).toImageMsg(); - lock.unlock(); - this->uvec_pub_.publish(uvec_msg); - got_uvec = true; - ROS_INFO("Got unit vectors, restarting framegrabber with mask: %d", - (int) this->schema_mask_); - - while (! this->InitStructures(this->schema_mask_)) - { - ROS_WARN("Could not re-initialize pixel stream!"); - ros::Duration(1.0).sleep(); - } - - // should solve the problem of first image being (0,0) - // see: https://github.com/lovepark/ifm3d/issues/12 - continue; - } - - // - // Pull out all the wrapped images so that we can release the "GIL" - // while publishing - // - lock.lock(); - - // boost::shared_ptr vs std::shared_ptr forces this copy - pcl::copyPointCloud(*(this->im_->Cloud().get()), *cloud); - xyz_img = this->im_->XYZImage(); - confidence_img = this->im_->ConfidenceImage(); - distance_img = this->im_->DistanceImage(); - amplitude_img = this->im_->AmplitudeImage(); - raw_amplitude_img = this->im_->RawAmplitudeImage(); - extrinsics = this->im_->Extrinsics(); - - lock.unlock(); - - // - // Now, do the publishing - // - - // Confidence image is invariant - no need to check the mask - sensor_msgs::ImagePtr confidence_msg = - cv_bridge::CvImage(optical_head, - "mono8", - confidence_img).toImageMsg(); - this->conf_pub_.publish(confidence_msg); - - if ((this->schema_mask_ & ifm3d::IMG_CART) == ifm3d::IMG_CART) - { - cloud->header = pcl_conversions::toPCL(head); - this->cloud_pub_.publish(cloud); - - sensor_msgs::ImagePtr xyz_image_msg = - cv_bridge::CvImage(head, - xyz_img.type() == CV_32FC3 ? - enc::TYPE_32FC3 : enc::TYPE_16SC3, - xyz_img).toImageMsg(); - this->xyz_image_pub_.publish(xyz_image_msg); - } - - if ((this->schema_mask_ & ifm3d::IMG_RDIS) == ifm3d::IMG_RDIS) - { - sensor_msgs::ImagePtr distance_msg = - cv_bridge::CvImage(optical_head, - distance_img.type() == CV_32FC1 ? - enc::TYPE_32FC1 : enc::TYPE_16UC1, - distance_img).toImageMsg(); - this->distance_pub_.publish(distance_msg); - } - - if ((this->schema_mask_ & ifm3d::IMG_AMP) == ifm3d::IMG_AMP) - { - sensor_msgs::ImagePtr amplitude_msg = - cv_bridge::CvImage(optical_head, - amplitude_img.type() == CV_32FC1 ? - enc::TYPE_32FC1 : enc::TYPE_16UC1, - amplitude_img).toImageMsg(); - this->amplitude_pub_.publish(amplitude_msg); - } - - if ((this->schema_mask_ & ifm3d::IMG_RAMP) == ifm3d::IMG_RAMP) - { - sensor_msgs::ImagePtr raw_amplitude_msg = - cv_bridge::CvImage(optical_head, - raw_amplitude_img.type() == CV_32FC1 ? - enc::TYPE_32FC1 : enc::TYPE_16UC1, - raw_amplitude_img).toImageMsg(); - this->raw_amplitude_pub_.publish(raw_amplitude_msg); - } - - // - // XXX: Need to publish ambient light / gray image - // ... however, as of now (3/26/2017) there is no - // imager mode on the O3X that actually supports it - // - // Update: as of 5/10/2018 still not supported. - // - - good_bad_pixels_img = cv::Mat::ones(confidence_img.rows, - confidence_img.cols, - CV_8UC1); - cv::bitwise_and(confidence_img, good_bad_pixels_img, - good_bad_pixels_img); - sensor_msgs::ImagePtr good_bad_msg = - cv_bridge::CvImage(optical_head, - "mono8", - (good_bad_pixels_img == 0)).toImageMsg(); - this->good_bad_pub_.publish(good_bad_msg); - - // - // publish extrinsics - // - ifm3d::Extrinsics extrinsics_msg; - extrinsics_msg.header = optical_head; - try - { - 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); - } - catch (const std::out_of_range& ex) - { - ROS_WARN("out-of-range error fetching extrinsics"); - } - this->extrinsics_pub_.publish(extrinsics_msg); - - } // end: while (ros::ok()) { ... } -} // end: Run() - - -PLUGINLIB_EXPORT_CLASS(ifm3d_ros::CameraNodelet, nodelet::Nodelet)