From 5a233c61969f349b86ab3f8d32807adac4633c9d Mon Sep 17 00:00:00 2001
From: Yuki Furuta <me@furushchev.ru>
Date: Tue, 25 Jun 2019 11:23:52 +0900
Subject: [PATCH 1/2] add node to get serial of device

---
 CMakeLists.txt |  5 +++-
 src/get_id.cpp | 67 ++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 71 insertions(+), 1 deletion(-)
 create mode 100644 src/get_id.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index a5f22489..fd6b00c0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -29,7 +29,10 @@ target_link_libraries(rplidarNode ${catkin_LIBRARIES})
 add_executable(rplidarNodeClient src/client.cpp)
 target_link_libraries(rplidarNodeClient ${catkin_LIBRARIES})
 
-install(TARGETS rplidarNode rplidarNodeClient
+add_executable(rplidarGetId src/get_id.cpp ${RPLIDAR_SDK_SRC})
+target_link_libraries(rplidarGetId ${catkin_LIBRARIES})
+
+install(TARGETS rplidarNode rplidarNodeClient rplidarGetId
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
diff --git a/src/get_id.cpp b/src/get_id.cpp
new file mode 100644
index 00000000..326e74a0
--- /dev/null
+++ b/src/get_id.cpp
@@ -0,0 +1,67 @@
+// -*- mode: C++ -*-
+/*
+ *  Copyright (c) 2019, GITAI Inc.
+ *  All rights reserved.
+ * get_id.cpp
+ * Author: Yuki Furuta <me@furushchev.ru>
+ */
+
+#include <cstdlib>
+#include <iomanip>
+#include <iostream>
+#include <rplidar.h>
+
+
+using namespace rp::standalone::rplidar;
+
+
+int main(int argc, char** argv)
+{
+  std::string serial_port = "/dev/ttyUSB0";
+  int serial_baudrate = 225600;
+
+  switch (argc)
+  {
+  case 2:
+    serial_port = argv[1];
+    break;
+  case 3:
+    serial_port = argv[1];
+    serial_baudrate = std::atoi(argv[2]);
+    break;
+  default:
+    std::cerr << "Usage: " << argv[0] << " [port] [baudrate]" << std::endl;
+    return 1;
+  }
+
+  RPlidarDriver* drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);
+  if (!drv)
+  {
+    std::cerr << "Failed to create driver instance" << std::endl;
+    return 1;
+  }
+
+  if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {
+    std::cerr << "Failed to connect to device" << std::endl;
+    RPlidarDriver::DisposeDriver(drv);
+    return -1;
+  }
+
+  rplidar_response_device_info_t devinfo;
+  u_result result = drv->getDeviceInfo(devinfo);
+  if (IS_FAIL(result))
+  {
+    if (result == RESULT_OPERATION_TIMEOUT)
+      std::cerr << "Operation timed out" << std::endl;
+    else
+      std::cerr << "Unexpected error (code: " << std::hex << result << ")" << std::endl;
+    return 1;
+  }
+
+  // print serial number
+  for (size_t i = 0; i < 16; ++i)
+    std::cout << std::setw(2) << std::setfill('0') << std::hex << devinfo.serialnum[i];
+  std::cout << std::endl;
+
+  return 0;
+}

From 2501aa7ba6f5ea95e7534e58c04f0baea912159e Mon Sep 17 00:00:00 2001
From: Yuki Furuta <y.furuta@gitai.tech>
Date: Tue, 25 Jun 2019 12:44:40 +0900
Subject: [PATCH 2/2] add node to print sensor serial number

---
 src/get_id.cpp | 24 ++++++++++++++----------
 1 file changed, 14 insertions(+), 10 deletions(-)

diff --git a/src/get_id.cpp b/src/get_id.cpp
index 326e74a0..1523927c 100644
--- a/src/get_id.cpp
+++ b/src/get_id.cpp
@@ -18,7 +18,8 @@ using namespace rp::standalone::rplidar;
 int main(int argc, char** argv)
 {
   std::string serial_port = "/dev/ttyUSB0";
-  int serial_baudrate = 225600;
+  int serial_baudrate = 256000;
+  RPlidarDriver* drv = NULL;
 
   switch (argc)
   {
@@ -30,11 +31,11 @@ int main(int argc, char** argv)
     serial_baudrate = std::atoi(argv[2]);
     break;
   default:
-    std::cerr << "Usage: " << argv[0] << " [port] [baudrate]" << std::endl;
+    std::cerr << "Usage: " << argv[0] << " [port (default: /dev/ttyUSB0)] [baudrate (default: 256000)]" << std::endl;
     return 1;
   }
 
-  RPlidarDriver* drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);
+  drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);
   if (!drv)
   {
     std::cerr << "Failed to create driver instance" << std::endl;
@@ -47,21 +48,24 @@ int main(int argc, char** argv)
     return -1;
   }
 
+  u_result     op_result;
   rplidar_response_device_info_t devinfo;
-  u_result result = drv->getDeviceInfo(devinfo);
-  if (IS_FAIL(result))
+  op_result = drv->getDeviceInfo(devinfo);
+
+  if (IS_FAIL(op_result))
   {
-    if (result == RESULT_OPERATION_TIMEOUT)
-      std::cerr << "Operation timed out" << std::endl;
-    else
-      std::cerr << "Unexpected error (code: " << std::hex << result << ")" << std::endl;
+    // serial number can be retrieved even if timed out.
+    std::cerr << "Failed to get device info (code: " << std::hex << op_result << ")" << std::endl;
+    RPlidarDriver::DisposeDriver(drv);
     return 1;
   }
 
   // print serial number
   for (size_t i = 0; i < 16; ++i)
-    std::cout << std::setw(2) << std::setfill('0') << std::hex << devinfo.serialnum[i];
+    std::cout << std::setw(2) << std::setfill('0') << std::hex << (int)devinfo.serialnum[i];
   std::cout << std::endl;
 
+  RPlidarDriver::DisposeDriver(drv);
+
   return 0;
 }