<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="20" />
  <param name="far_signal_cutoff" value="0.2" />
  <param name="camera_pixel" value="320x240" />
  <param name="ir_gain" value="8" />
  <param name="distance_mode" value="dm_1_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: -1.5708 # -M_PI/2
            angle_max: 1.5708 # M_PI/2
            angle_increment: 0.0087 # M_PI/360.0
            scan_time: 0.3333
      range_min: 0.7
            range_max: 6.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.4"/>
    <param name="map_update_angle_thresh" value="0.06" />
    <param name="laser_z_min_value" value = "-1.0" />
    <param name="laser_z_max_value" value = "1.0" />
    <!-- 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.05" />             
    <!-- 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="6.0" />
    <remap from="cloud_in" to="/cloud" />       
  </node>
</launch>