# Blickfeld ROS2 package This package provides a ROS2 node for publishing PointCloud2 messages from Blickfeld LiDAR devices. The driver is available under https://www.blickfeld.com/resources ## Supported devices The Blickfeld ROS2 driver supports all currently available Blickfeld LiDARs such as Cube 1 and Cube Range 1. ## Supported ROS2 Distributions The Blickfeld ROS2 driver supports the following ROS2 Distribution - Foxy Fitzroy ## obtain ``` git clone --recursive ``` This will clone ros_blickfeld_driver. ## Dependencies As submodule ros_blickfeld_driver depends on: - ros_blickfeld_driver_core v0.2.3 (resides in modules directory) To install the Blickfeld ROS2 driver, please make sure the following dependencies are installed on your system. - [blickfeld-scanner-library (BSL) ](https://docs.blickfeld.com/cube/latest/external/blickfeld-scanner-lib/install.html) with system-wide protobuf installation. This package has been tested using **BSL Version 2.18.2** - [ROS Foxy Installation](https://index.ros.org/doc/ros2/Installation/Foxy/Linux-Install-Debians/) with Ubuntu 20.04 - [diagnostic_updater](https://index.ros.org/p/diagnostic_updater/) can be acquired via your distribution's package manager, ${ROS_DISTRO} should be your ROS 2 version. (foxy) $ sudo apt install ros-${ROS_DISTRO}-diagnostic-updater $ sudo apt install ros-${ROS_DISTRO}-diagnostic-msgs or via $ rosdep update $ rosdep install --from-paths src --ignore-src -r -y --skip-keys "blickfeld-scanner" The -skip-keys will instruct rosdep to not check blickfeld-scanner, which is not a ros package. - [colcon installation](https://colcon.readthedocs.io/en/released/user/installation.html): required for building the workspace The default DDS of ROS2 Foxy **FastRTPS** is not compatible with BSL. Hence for ROS2 Foxy, we need to install DDS from other vendors. (e.g. Eclipse Cyclone or RTI Connext) - [Eclipse Cyclone DDS](https://index.ros.org/doc/ros2/Installation/DDS-Implementations/Working-with-Eclipse-CycloneDDS/) to bypass the default DDS of ROS2 Foxy(FastRTPS) ## Build Before building ensure that your ROS DISTRO is sourced. You will need to run this command on every new shell you open to have access to the ROS 2 commands. ${ROS_DISTRO} should be your ROS 2 version. (e.g. foxy) $ source /opt/ros/${ROS_DISTRO}/setup.bash To build the ROS2 driver, request the ROS2 driver from the Blickfeld Sales department and decompress the archive into your ROS2 workspace and run. $ colcon build --symlink-install --cmake-clean-first ## Switching to desired DDS For **ROS2 Foxy, we need to switch from the default DDS to a compatible one**. This can be done using **either of the two** approaches. 1. Exporting the RMW_IMPLEMENTATION for the present terminal session ``` $ export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp ``` 2. [Switching to the desired RMW_IMPLEMENTATION](https://index.ros.org/doc/ros2/Tutorials/Working-with-multiple-RMW-implementations/) while running by adding a prefix before the ros2 run ``` $ RMW_IMPLEMENTATION=rmw_cyclonedds_cpp ros2 run PKG_NAME NODE_NAME ARGUMENTS ``` **NOTE** Before starting a communication with blickfeld driver, the default DDS should be changed to cyclone DDS or other vendors. Therefore it is suggested to set a system global variable for `RMW_IMPLEMENTATION=rmw_cyclonedds_cpp`. ## Running the Blickfeld ROS node Before running, ensure that your colcon workspace is sourced. This is achieved by sourcing the setup script in your colcon workspace. $ source install/setup.bash You can launch the ROS driver in a DHCP controlled Network by providing the Hostname of the LiDAR device (you can check and set the Hostname in the WebGUI of the device) e.g: $ ros2 run blickfeld_driver blickfeld_driver_node --ros-args -p host:=cube-0175 --remap __node:=bf_lidar If you connect the LiDAR directly to your PC or don't have a DHCP Server you can launch the driver by using the IP configured in the WebGUI of the Device or the Fallback-IP of the Device (**make sure to adjust your network-settings accordingly**) e.g: $ ros2 run blickfeld_driver blickfeld_driver_node --ros-args -p host:=cube-0175 --remap __node:=bf_lidar You can also start it with more parameters, e.g.: $ ros2 run blickfeld_driver blickfeld_driver_node --ros-args -p host:=cube-0175 -p publish_ambient_light:=true -p publish_intensities:=false To visualize the published data, run rviz2 $ ros2 run rviz2 rviz2 If you prefer a similar way to **launch**, like you would in ROS(1) (beware of the new ".xml"), you can start the driver like this: $ ros2 launch -a blickfeld_driver live_scanner.launch.xml host:=cube-0175 Or if you prefer to put the parameters into a yaml, run the python launch file. It reads the two yaml file containing the configurations. $ ros2 launch blickfeld_driver live_scanner_yaml.launch.py The yaml files are located in the **ros2_blickfeld_driver/config** folder of the blickfeld_driver package. There are two yaml files: 1. driver_config.yaml Configures host id, output point cloud topic etc. 2. blickfeld_scanner.rviz Configures the parameters for Rviz visualization, e.g. topic of point cloud, configuration of opened viewers etc. ### Parameters All the following parameters are available in launch _and_ run: | Argument | Default | Note | | ------------------------------------------------ | -------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | | **host** (required) | | The host name or the IP of the device you want to publish the point clouds from, e.g., `cube-0175`. | | lidar_frame_id | `lidar` | The ROS TF where the point cloud should be in. | | node_name | `bf_lidar` | Name of this ROS node. | | remap | `true` | Remap this node’s input/output topics to commonly used ones.
If `false`, canonical names in the form of `$(arg node_name)/foo*(in/out)`are used, depending on whether a topic is an input or and output topic. | | rviz | `false` | Start rviz if this argument is true. | | publish_ambient_light | `false` | Set to true if the PointCloud2 message should contain the ambient light level. | | publish_explicit_range | `false` | Set to true if the PointCloud2 message should contain the`range` field. | | publish_intensities | `true` | Set to true if the PointCloud2 message should contain intensities. | | publish_no_return_points | `false` | Set to true if the PointCloud2 message should contain points in a given range for pulses without a return. | | publish_point_id | `false` | Add the `scanline_id` field, the `scanline_point_index` field (= the point’s number in the scan line), and the `point_id` (= frame-global point ID) to PointCloud2 message.
If the device is configured to return multiple return points (multiple reflections), all these three IDs will be identical; only the `return_id` will differ. | | publish_point_time_offset | `false` | If `true`, the PointCloud2 message will contain the timestamp field per point to represent the time offset from the start of the frame. | | no_return_point_range | `1.0` | Dummy range for points of pulses without a return. | | returns_publishing_options | `strongest` | different options to publish multiple returns, possible values are: `strongest` `closest` `farthest` and `all`, in case of `all`, the field `return_id` (ID for each returned point) is added to the point cloud. | | projection_type | `angle_preserving` | different options to project the data onto a 2D image, possible values are: `angle_preserving` or `scanline_preserving`. These indicate if a correct spehrical projection is desired, or each row of the image should correspond to one scanline. | | publish_ambient_image | `false` | Enables publishing of am ambient light image. | | publish_intensity_image | `false` | Enables publishing of an intensity image. | | publish_range_image | `false` | Enables publishing of a range image. | | publish_imu | `false` | Enables publishing of IMU data in burst mode. | | publish_imu_static_tf_at_start | `false` | Call "Publish IMU tf transform" service once at the beginning. | | use_lidar_timestamp | `true` | Set to true if the timestamp in the ROS point cloud message should be generated from the device timestamp; otherwise the timestamp will be the ROS time when the frame was received on ROS. | | use_background_subtraction | `false` | Enables on-device background subtraction. | | use_neighbor_filter | `false` | Enables on-device neighbor filter. | | background_subtraction_exponential_decay_rate | `0.005` | Controls how fast objects switch between foreground and background. Exponential decay factor. | | background_subtraction_num_initialization_frames | `10` | Number of frames to initialize the background with. | | ambient_image_out | `$(arg node_name)/ambient_image_out` | Topic to publish the ambient image on. | | diagnostic_out | `$(arg node_name)/diagnostic` | Topic to publish the diagnostic status. | | imu_out | `$(arg node_name)/imu` | Topic to publish the IMU burst data. | | intensity_image_out | `$(arg node_name)/intensity_image_out` | Topic to publish the intensity image on. | | point_cloud_out | `$(arg node_name)/points_raw` | Topic to publish the PointCloud2 message to. | | range_image_out | `$(arg node_name)/range_image_out` | Topic to publish the range image on. | ### Debugging Add '\_\_log_level:=debug' to see details: $ ros2 run blickfeld_driver blickfeld_driver_node --ros-args --remap __node:=bf_lidar -p host:=cube-0175 -p publish_ambient_light:=true -p publish_no_return_points:=true __log_level:=debug #### ROS2 message content To look at the ROS message sent by this ROS node, you can use $ debugging_tools/ros2-topic-echoparsed.py It shows all the fields present in the message. ## Services The Blickfeld ROS2 driver advertises different services. For service call change the [default DDS](#switching-to-desired-dds) from FastRTPS. ### Set scan pattern Before running, ensure that your colcon workspace is sourced. This service call will set scan pattern on a device. e.g. `ros2 service call /$(arg node_name)/set_scan_pattern blickfeld_driver/srv/SetScanPattern "{fov_horizontal: 72.0, fov_vertical: 30.0, angle_spacing: 0.4, scanlines_up: 28, scanlines_down: 28, frame_mode: 'COMBINE_UP_DOWN', pulse_type: 'INTERLEAVE'}" ` ### Publish IMU tf transform This service call will receive the static IMU data from device and creates a frame from the data with the frame_id `$(arg lidar_frame_id)_imu`. `ros2 service call /$(arg node_name)/publish_imu_static_tf_service blickfeld_driver/srv/ImuStaticTransformation` ## License This package is released under a [BSD 3-Clause License](LICENSE) (see also [https://opensource.org/licenses/BSD-3-Clause](https://opensource.org/licenses/BSD-3-Clause))