From 95d935f2edfda6c4be3e1d767caf54195f13166c Mon Sep 17 00:00:00 2001
From: WubinXia <wubin.xia@slamtec.com>
Date: Fri, 15 Mar 2019 16:08:29 +0800
Subject: [PATCH] Support TCP

---
 launch/rplidar_s1.launch          |  3 +--
 launch/rplidar_s1_tcp.launch      | 10 ++++++++
 launch/view_rplidar_s1_tcp.launch | 10 ++++++++
 src/node.cpp                      | 38 +++++++++++++++++++++++++------
 4 files changed, 52 insertions(+), 9 deletions(-)
 create mode 100644 launch/rplidar_s1_tcp.launch
 create mode 100644 launch/view_rplidar_s1_tcp.launch

diff --git a/launch/rplidar_s1.launch b/launch/rplidar_s1.launch
index 11753abd..26668545 100644
--- a/launch/rplidar_s1.launch
+++ b/launch/rplidar_s1.launch
@@ -1,10 +1,9 @@
 <launch>
   <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
   <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
-  <param name="serial_baudrate"     type="int"    value="256000"/><!--A3 -->
+  <param name="serial_baudrate"     type="int"    value="256000"/>
   <param name="frame_id"            type="string" value="laser"/>
   <param name="inverted"            type="bool"   value="false"/>
   <param name="angle_compensate"    type="bool"   value="true"/>
-  <param name="scan_mode"           type="string" value="Express"/>
   </node>
 </launch>
diff --git a/launch/rplidar_s1_tcp.launch b/launch/rplidar_s1_tcp.launch
new file mode 100644
index 00000000..a1e6b4bd
--- /dev/null
+++ b/launch/rplidar_s1_tcp.launch
@@ -0,0 +1,10 @@
+<launch>
+  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
+  <param name="channel_type"        type="string" value="tcp"/>  
+  <param name="tcp_ip"              type="string" value="192.168.0.7"/>  
+  <param name="tcp_port"            type="int"    value="20108"/>
+  <param name="frame_id"            type="string" value="laser"/>
+  <param name="inverted"            type="bool"   value="false"/>
+  <param name="angle_compensate"    type="bool"   value="true"/>
+  </node>
+</launch>
diff --git a/launch/view_rplidar_s1_tcp.launch b/launch/view_rplidar_s1_tcp.launch
new file mode 100644
index 00000000..88b011df
--- /dev/null
+++ b/launch/view_rplidar_s1_tcp.launch
@@ -0,0 +1,10 @@
+<!--
+  Used for visualising rplidar in action.  
+  
+  It requires rplidar.launch.
+ -->
+<launch>
+  <include file="$(find rplidar_ros)/launch/rplidar_s1_tcp.launch" />
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find rplidar_ros)/rviz/rplidar.rviz" />
+</launch>
diff --git a/src/node.cpp b/src/node.cpp
index d3442ea0..4b12ea6b 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -181,8 +181,11 @@ static float getAngle(const rplidar_response_measurement_node_hq_t& node)
 
 int main(int argc, char * argv[]) {
     ros::init(argc, argv, "rplidar_node");
-
+    
+    std::string channel_type;
+    std::string tcp_ip;
     std::string serial_port;
+    int tcp_port = 20108;
     int serial_baudrate = 115200;
     std::string frame_id;
     bool inverted = false;
@@ -193,6 +196,9 @@ int main(int argc, char * argv[]) {
     ros::NodeHandle nh;
     ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000);
     ros::NodeHandle nh_private("~");
+    nh_private.param<std::string>("channel_type", channel_type, "serial");
+    nh_private.param<std::string>("tcp_ip", tcp_ip, "192.168.0.7"); 
+    nh_private.param<int>("tcp_port", tcp_port, 20108);
     nh_private.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0"); 
     nh_private.param<int>("serial_baudrate", serial_baudrate, 115200/*256000*/);//ros run for A1 A2, change to 256000 if A3
     nh_private.param<std::string>("frame_id", frame_id, "laser_frame");
@@ -205,20 +211,38 @@ int main(int argc, char * argv[]) {
     u_result     op_result;
 
     // create the driver instance
-    drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT);
+    if(channel_type == "tcp"){
+        drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_TCP);
+    }
+    else{
+        drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT);
+    }
+
     
     if (!drv) {
         ROS_ERROR("Create Driver fail, exit");
         return -2;
     }
 
-    // make connection...
-    if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {
-        ROS_ERROR("Error, cannot bind to the specified serial port %s.",serial_port.c_str());
-        RPlidarDriver::DisposeDriver(drv);
-        return -1;
+    if(channel_type == "tcp"){
+        // make connection...
+        if (IS_FAIL(drv->connect(tcp_ip.c_str(), (_u32)tcp_port))) {
+            ROS_ERROR("Error, cannot bind to the specified serial port %s.",serial_port.c_str());
+            RPlidarDriver::DisposeDriver(drv);
+            return -1;
+        }
+
     }
+    else{
+       // make connection...
+        if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {
+            ROS_ERROR("Error, cannot bind to the specified serial port %s.",serial_port.c_str());
+            RPlidarDriver::DisposeDriver(drv);
+            return -1;
+        }
 
+    }
+    
     // get rplidar device info
     if (!getRPLIDARDeviceInfo(drv)) {
         return -1;