From ff11fc09407989cf3f5c19c8e1667ade7a93924a Mon Sep 17 00:00:00 2001
From: Osenberg <junyangqu@slightech.com>
Date: Wed, 27 Mar 2019 17:23:13 +0800
Subject: [PATCH] feat(ros): added run laserscan launch file for S1030 device

---
 .../config/laserscan/s1030_laserscan.yaml     |   4 +
 .../laserscan/mynteye_s1030_laserscan.launch  | 157 ++++++++++++++++++
 .../launch/mynteye.launch                     |   5 +
 .../src/wrapper_nodelet.cc                    |  45 +++--
 4 files changed, 200 insertions(+), 11 deletions(-)
 create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/config/laserscan/s1030_laserscan.yaml
 create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/launch/laserscan/mynteye_s1030_laserscan.launch

diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/config/laserscan/s1030_laserscan.yaml b/wrappers/ros/src/mynt_eye_ros_wrapper/config/laserscan/s1030_laserscan.yaml
new file mode 100644
index 0000000..7818e14
--- /dev/null
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/config/laserscan/s1030_laserscan.yaml
@@ -0,0 +1,4 @@
+# is_laserscan
+is_laserscan: true
+
+
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/laserscan/mynteye_s1030_laserscan.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/laserscan/mynteye_s1030_laserscan.launch
new file mode 100644
index 0000000..9883720
--- /dev/null
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/laserscan/mynteye_s1030_laserscan.launch
@@ -0,0 +1,157 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="mynteye" default="mynteye" />
+
+  <!-- mutiple device -->
+  <arg name="is_multiple" default="false" />
+
+  <!-- serial_number -->
+  <!-- if is_mutiple is true, must set serial_number -->
+  <arg name="serial_number" default="" />
+
+  <!-- depth_type  0: MONO16, 1: TYPE_16UC1 -->
+  <arg name="depth_type" default="1" />
+
+  <!-- node params -->
+
+  <arg name="left_topic" default="left/image_raw" />
+  <arg name="right_topic" default="right/image_raw" />
+  <arg name="left_rect_topic" default="left_rect/image_rect" />
+  <arg name="right_rect_topic" default="right_rect/image_rect" />
+  <arg name="disparity_topic" default="disparity/image_raw" />
+  <arg name="disparity_norm_topic" default="disparity/image_norm" />
+  <arg name="depth_topic" default="depth/image_raw" />
+  <arg name="points_topic" default="points/data_raw" />
+
+  <arg name="left_mono_topic" default="left/image_mono" />
+  <arg name="right_mono_topic" default="right/image_mono" />
+
+  <arg name="imu_topic" default="imu/data_raw" />
+  <arg name="temperature_topic" default="temperature/data_raw" />
+
+  <arg name="base_frame_id" default="$(arg mynteye)_link" />
+  <arg name="left_frame_id" default="$(arg mynteye)_left_frame" />
+  <arg name="right_frame_id" default="$(arg mynteye)_right_frame" />
+  <arg name="left_rect_frame_id" default="$(arg mynteye)_left_rect_frame" />
+  <arg name="right_rect_frame_id" default="$(arg mynteye)_right_rect_frame" />
+  <arg name="disparity_frame_id" default="$(arg mynteye)_disparity_frame" />
+  <arg name="disparity_norm_frame_id" default="$(arg mynteye)_disparity_norm_frame" />
+  <arg name="points_frame_id" default="$(arg mynteye)_points_frame" />
+  <arg name="depth_frame_id" default="$(arg mynteye)_depth_frame" />
+
+  <arg name="temperature_frame_id" default="$(arg mynteye)_temperature_frame" />
+
+  <arg name="gravity" default="9.8" />
+
+  <arg name="mesh_file" default="S1030-0315.obj" />
+  <!-- Push down all topics/nodelets into "mynteye" namespace -->
+  <group ns="$(arg mynteye)">
+
+    <!-- mynteye_wrapper_node -->
+    <node name="mynteye_wrapper_node" pkg="mynt_eye_ros_wrapper" type="mynteye_wrapper_node" output="screen" respawn="true" respawn_delay="5">
+
+      <!-- node params -->
+
+      <param name="is_multiple" value="$(arg is_multiple)" />
+      <param name="serial_number" value="$(arg serial_number)" />
+
+      <param name="depth_type" value="$(arg depth_type)" />
+
+      <param name="left_topic" value="$(arg left_topic)" />
+      <param name="right_topic" value="$(arg right_topic)" />
+      <param name="left_rect_topic" value="$(arg left_rect_topic)" />
+      <param name="right_rect_topic" value="$(arg right_rect_topic)" />
+      <param name="disparity_topic" value="$(arg disparity_topic)" />
+      <param name="disparity_norm_topic" value="$(arg disparity_norm_topic)" />
+      <param name="points_topic" value="$(arg points_topic)" />
+      <param name="depth_topic" value="$(arg depth_topic)" />
+      <param name="mesh_file" value="$(arg mesh_file)" />
+
+      <param name="left_mono_topic" value="$(arg left_mono_topic)" />
+      <param name="right_mono_topic" value="$(arg right_mono_topic)" />
+
+      <param name="imu_topic" value="$(arg imu_topic)" />
+      <param name="temperature_topic" value="$(arg temperature_topic)" />
+
+      <param name="base_frame_id" value="$(arg base_frame_id)" />
+      <param name="left_frame_id" value="$(arg left_frame_id)" />
+      <param name="right_frame_id" value="$(arg right_frame_id)" />
+      <param name="left_rect_frame_id" value="$(arg left_rect_frame_id)" />
+      <param name="right_rect_frame_id" value="$(arg right_rect_frame_id)" />
+      <param name="disparity_frame_id" value="$(arg disparity_frame_id)" />
+      <param name="disparity_norm_frame_id" value="$(arg disparity_norm_frame_id)" />
+      <param name="points_frame_id" value="$(arg points_frame_id)" />
+      <param name="depth_frame_id" value="$(arg depth_frame_id)" />
+
+      <param name="temperature_frame_id" value="$(arg temperature_frame_id)" />
+
+      <rosparam file="$(find mynt_eye_ros_wrapper)/config/device/standard.yaml" command="load" />
+      <rosparam file="$(find mynt_eye_ros_wrapper)/config/device/standard2.yaml" command="load" />
+      <rosparam file="$(find mynt_eye_ros_wrapper)/config/process/process_config.yaml" command="load" />
+
+      <rosparam file="$(find mynt_eye_ros_wrapper)/config/laserscan/s1030_laserscan.yaml" command="load" />
+
+      <param name="gravity" value="$(arg gravity)" />
+    </node>
+
+    <!-- disable compressed depth plugin for image topics -->
+    <group ns="$(arg left_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg left_rect_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg left_mono_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg right_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg right_mono_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg right_rect_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg disparity_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg disparity_norm_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg depth_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+  </group> <!-- mynteye -->
+
+  <node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan_loader" args="standalone depthimage_to_laserscan/DepthImageToLaserScanNodelet" output="screen">
+    <param name="scan_height" value="1" />
+    <param name="output_frame_id" value="depth_to_laserscan" />
+    <param name="range_min" value="0.45" />
+    <param name="range_max" value="10" />
+    <remap from="image" to="/mynteye/depth/image_raw" />
+    <remap from="camera_info" to="/mynteye/depth/camera_info" />
+  </node>
+
+  <node pkg="tf" type="static_transform_publisher" name="transform_coordinate" args="1 0 0 0 0 0 1 base_link depth_to_laserscan 100">
+  </node>
+
+</launch>
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
index 79615a5..4420f1d 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
@@ -9,6 +9,9 @@
   <!-- if is_mutiple is true, must set serial_number -->
   <arg name="serial_number" default="" />
 
+  <!-- depth_type  0: MONO16, 1: TYPE_16UC1 -->
+  <arg name="depth_type" default="0" />
+
   <!-- node params -->
 
   <arg name="left_topic" default="left/image_raw" />
@@ -52,6 +55,8 @@
       <param name="is_multiple" value="$(arg is_multiple)" />
       <param name="serial_number" value="$(arg serial_number)" />
 
+      <param name="depth_type" value="$(arg depth_type)" />
+
       <param name="left_topic" value="$(arg left_topic)" />
       <param name="right_topic" value="$(arg right_topic)" />
       <param name="left_rect_topic" value="$(arg left_rect_topic)" />
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
index dd01f41..4f319ab 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
@@ -324,6 +324,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       }
     }
 
+    int depth_type = 0;
+    private_nh_.getParamCached("depth_type", depth_type);
     if (model_ == Model::STANDARD2 || model_ == Model::STANDARD210A) {
       camera_encodings_ = {{Stream::LEFT, enc::BGR8},
                           {Stream::RIGHT, enc::BGR8},
@@ -334,13 +336,23 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
                           {Stream::DEPTH, enc::MONO16}};
     }
     if (model_ == Model::STANDARD) {
-      camera_encodings_ = {{Stream::LEFT, enc::MONO8},
-                          {Stream::RIGHT, enc::MONO8},
-                          {Stream::LEFT_RECTIFIED, enc::MONO8},
-                          {Stream::RIGHT_RECTIFIED, enc::MONO8},
-                          {Stream::DISPARITY, enc::MONO8},  // float
-                          {Stream::DISPARITY_NORMALIZED, enc::MONO8},
-                          {Stream::DEPTH, enc::MONO16}};
+      if (depth_type == 0) {
+        camera_encodings_ = {{Stream::LEFT, enc::MONO8},
+          {Stream::RIGHT, enc::MONO8},
+          {Stream::LEFT_RECTIFIED, enc::MONO8},
+          {Stream::RIGHT_RECTIFIED, enc::MONO8},
+          {Stream::DISPARITY, enc::MONO8},  // float
+          {Stream::DISPARITY_NORMALIZED, enc::MONO8},
+          {Stream::DEPTH, enc::MONO16}};
+      } else if (depth_type == 1) {
+        camera_encodings_ = {{Stream::LEFT, enc::MONO8},
+          {Stream::RIGHT, enc::MONO8},
+          {Stream::LEFT_RECTIFIED, enc::MONO8},
+          {Stream::RIGHT_RECTIFIED, enc::MONO8},
+          {Stream::DISPARITY, enc::MONO8},  // float
+          {Stream::DISPARITY_NORMALIZED, enc::MONO8},
+          {Stream::DEPTH, enc::TYPE_16UC1}};
+      }
     }
     pub_imu_ = nh_.advertise<sensor_msgs::Imu>(imu_topic, 100);
     NODELET_INFO_STREAM("Advertized on topic " << imu_topic);
@@ -1312,10 +1324,21 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
         }
       } else {
         if (info_pair->left.distortion_model == "KANNALA_BRANDT") {
-          camera_info->distortion_model =
-            sensor_msgs::distortion_models::EQUIDISTANT;
-          for (size_t i; i < 4; i++) {
-            camera_info->D.push_back(info_pair->left.D[i]);
+          // compatible laserscan
+          bool is_laserscan = false;
+          private_nh_.getParamCached("is_laserscan", is_laserscan);
+          if (!is_laserscan) {
+            camera_info->distortion_model =
+              sensor_msgs::distortion_models::EQUIDISTANT;
+            for (size_t i; i < 4; i++) {
+              camera_info->D.push_back(info_pair->left.D[i]);
+            }
+          } else {
+            camera_info->distortion_model =
+              sensor_msgs::distortion_models::PLUMB_BOB;
+            for (size_t i; i < 4; i++) {
+              camera_info->D.push_back(0.0);
+            }
           }
         } else if (info_pair->left.distortion_model == "PINHOLE") {
           camera_info->distortion_model =