Package Summary

ROS package for HLDS(Hitachi-LG Data Storage,Inc.) HLS-LFOM 3D LiDAR(TOF)

Manufacturer of the TOF: Hitachi-LG Data Storage, Inc.

Product Webpage: HLDS 3D LiDAR(TOF)

Overview

* Conducts real-time sensing of the distance object image to obtain 3-dimensional data

* 3D data and QVGA IR image can be simultaneously outputted

* Light Emitting part: Near Infrared LD, Light Receiving Part: CCD image sensor

HLS-LFOM1 view_hlds_tof_driver.launch

Product Specification

produc specification

Installation

Download HLS-LFOM SDK

Download latest version(v2.1.0 or later) for Ubuntu 16.04 LTS (x64) and extract it(http://hlds.co.jp/product-eng/tofsdk)

Install SDK

$ sudo dpkg -i libtof-dev_<version_number>ubuntu16_amd64.deb

Build HLS-LFOM 3D LiDAR(TOF) driver

$ cd ~/catkin_ws/src  
$ git clone https://github.com/HLDS-GIT/hls_lfom_tof_driver.git  
$ cd ~/catkin_ws  
$ catkin_make

Setting IP address

Modify IP address of tof.ini file in launch folder. It should be the same as the IP address of the TOF sensor you are trying to connect. (Default is 192.168.0.105)
If you want to change IP address, please refer to Setup TOF sensor of API Reference Manual in SDK

Running Node

Run hlds_3dtof_node

roslaunch hls_lfom_tof_driver hlds_3dtof.launch  

Run hlds_3dtof_node with RViz

roslaunch hls_lfom_tof_driver view_hlds_3dtof.launch  

Nodes

hlds_3dtof_node

hlds_3dtof_node is a driver for HLS-LFOM 3D LiDAR(TOF). It reads raw sensor data using HLS-LFOM's SDK and convert to ROS message.

Published Topics

cloud (sensor_msgs/PointCloud2)
  • XYZ point cloud
image_depth_raw (sensor_msgs/Image)
  • point.z data with TYPE_32FC1 encoding type
image_depth (sensor_msgs/Image)
  • 3channel depths data with RGB8 encoding type
image_ir (sensor_msgs/Image)
  • IR image with MONO8 encoding type

Parameters

camera_mode (string, default: CameraModeDepth) sensor_angle_x (int, default: 0)
  • X-axis rotation angle(0 to 359 degree)
sensor_angle_y (int, default: 0)
  • Y-axis rotation angle(0 to 359 degree)
sensor_angle_z (int, default: 0)
  • Y-axis rotation angle(0 to 359 degree)
edge_signal_cutoff (bool, default: true)
  • Edge noise reduction mode
low_signal_cutoff (int, default: 20)
  • Signal level to cutoff(0 to 4095)
far_signal_cutoff (float, default: 0.0)
  • Distance rate far from TOF(rate:0.0~1.0)
camera_pixel (string, default: 320x240)
  • Pixel size of TOF sensor image. Refer to CameraPixel type of API reference manual in SDK
ir_gain (int, default: 8)
  • Exposure sensitivity for IR
distance_mode (string, default: dm_1_0x)
  • Distance mode of TOF sensor. Refer to DistanceMode type of API reference manual in SDK
frame_rate (string, default: fr30fps)
  • Frame rate of TOF sensor. Refer to FrameRate type of API reference manual in SDK

Use Case

3D SLAM

produc specification

Package Installation

// Install pointcloud to laserscan
$ sudo apt-get install ros-kinetic-pointcloud-to-laserscan
//Install hector slam
$ sudo apt-get install ros-kinetic-hector-slam
//Install octomap
$ sudo apt-get install ros-kinetic-octomap-server

Example

<launch>
  <node pkg="hls_lfom_tof_driver" type="hlds_3dtof_node" name="hls_lfom_tof_driver"
        args="" required="true" output="screen" >
  </node>
  <param name="ini_path" type="str" value="$(find hls_lfom_tof_driver)/launch/" />
  <param name="frame_id" value="tof_link" />
  <param name="cloud_topic" value="cloud" />
  <param name="camera_mode" value="CameraModeDepth" />
  <param name="sensor_angle_x" value="270" />
  <param name="sensor_angle_y" value="0" />
  <param name="sensor_angle_z" value="270" />
  <param name="edge_signal_cutoff" value="true" />
  <param name="low_signal_cutoff" value="15" />
  <param name="far_signal_cutoff" value="0.1" />
  <param name="camera_pixel" value="320x240" />
  <param name="ir_gain" value="8" />
  <param name="distance_mode" value="dm_2_0x" />
  <param name="frame_rate" value="fr30fps" />

  <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
    <remap from="cloud_in" to="cloud"/>
    <remap from="scan" to="scan"/>
    <rosparam>
      transform_tolerance: 0.01
      min_height: 0.0
      max_height: 1.0
      angle_min: -0.7854 # -1.5708 # -M_PI/2
      angle_max: 0.7854 # 1.5708 # M_PI/2
      angle_increment: 0.0043 #0.0087 # M_PI/360.0
      scan_time: 0.0333 # 0.3333
      range_min: 0.7
      range_max: 10.0
      use_inf: true
      concurrency_level: 1
    </rosparam>
  </node>

  <arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
  <arg name="base_frame" default="base_link"/>
  <arg name="odom_frame" default="base_link"/>
  <arg name="pub_map_odom_transform" default="true"/>
  <arg name="scan_subscriber_queue_size" default="5"/>
  <arg name="scan_topic" default="scan"/>
  <arg name="map_size" default="2048"/>  
  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">    
    <!-- Frame names -->
    <param name="map_frame" value="map" />
    <param name="base_frame" value="$(arg base_frame)" />
    <param name="odom_frame" value="$(arg odom_frame)" />    
    <!-- Tf use -->
    <param name="use_tf_scan_transformation" value="true"/>
    <param name="use_tf_pose_start_estimate" value="false"/>
    <param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>    
    <!-- Map size / start point -->
    <param name="map_resolution" value="0.050"/>
    <param name="map_size" value="$(arg map_size)"/>
    <param name="map_start_x" value="0.5"/>
    <param name="map_start_y" value="0.5" />
    <param name="map_multi_res_levels" value="2" />
    <!-- Map update parameters -->
    <param name="update_factor_free" value="0.4"/>
    <param name="update_factor_occupied" value="0.9" />    
    <param name="map_update_distance_thresh" value="0.1"/>
    <param name="map_update_angle_thresh" value="0.04" />
    <param name="laser_z_min_value" value = "-0.1" />
    <param name="laser_z_max_value" value = "0.1" />    
    <!-- Advertising config --> 
    <param name="advertise_map_service" value="true"/>    
    <param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
    <param name="scan_topic" value="$(arg scan_topic)"/>
    <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
  </node>

  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    <param name="resolution" value="0.+" />             
    <!-- fixed map frame (set to 'map' if SLAM or localization running!) -->
    <param name="frame_id" type="string" value="map" />         
    <!-- maximum range to integrate (speedup!) -->
    <param name="sensor_model/max_range" value="7.0" />
    <remap from="cloud_in" to="/cloud" />       
  </node>
</launch>

HLDS 2D LiDAR: HLS-LFCD2 HLS-LFCD3

Wiki: hls_lfom_tof_driver (last edited 2018-06-04 07:14:34 by JeehoonYang)