From 4f899e670bec2c9e1f26b0969f2de36d23618ef3 Mon Sep 17 00:00:00 2001
From: yzx <wubin.xia@slamtec.com>
Date: Tue, 31 Dec 2019 10:18:02 +0800
Subject: [PATCH] bugfix:start/stop_motor(service) cause lidar to stop scanning

---
 sdk/src/rplidar_driver.cpp    |  4 +++-
 sdk/src/rplidar_driver_impl.h |  3 +--
 src/node.cpp                  | 14 ++++++++------
 3 files changed, 12 insertions(+), 9 deletions(-)

diff --git a/sdk/src/rplidar_driver.cpp b/sdk/src/rplidar_driver.cpp
index 9b0b532e..65d2c22a 100644
--- a/sdk/src/rplidar_driver.cpp
+++ b/sdk/src/rplidar_driver.cpp
@@ -188,7 +188,6 @@ u_result RPlidarDriverImplCommon::_waitResponseHeader(rplidar_ans_header_t * hea
 }
 
 
-
 u_result RPlidarDriverImplCommon::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout)
 {
     u_result  ans;
@@ -1550,6 +1549,7 @@ u_result RPlidarDriverImplCommon::getScanModeCount(_u16& modeCount, _u32 timeout
 
 u_result RPlidarDriverImplCommon::startScan(bool force, bool useTypicalScan, _u32 options, RplidarScanMode* outUsedScanMode)
 {
+    if(_isScanning)return RESULT_ALREADY_DONE;
     u_result ans;
 
     bool ifSupportLidarConf = false;
@@ -2158,6 +2158,7 @@ u_result RPlidarDriverImplCommon::setMotorPWM(_u16 pwm)
     return RESULT_OK;
 }
 
+
 u_result RPlidarDriverImplCommon::setLidarSpinSpeed(_u16 rpm, _u32 timeout)
 {
     if (!_isTofLidar) return RESULT_OPERATION_NOT_SUPPORT;
@@ -2188,6 +2189,7 @@ u_result RPlidarDriverImplCommon::startMotor()
     }
     else {
         setLidarSpinSpeed(600);//set default rpm to tof lidar
+        return RESULT_OK;
     }
 
 }
diff --git a/sdk/src/rplidar_driver_impl.h b/sdk/src/rplidar_driver_impl.h
index 950194ff..1cbaca88 100644
--- a/sdk/src/rplidar_driver_impl.h
+++ b/sdk/src/rplidar_driver_impl.h
@@ -59,7 +59,6 @@ namespace rp { namespace standalone{ namespace rplidar {
     virtual u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL);
     virtual u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _u32 timeout = DEFAULT_TIMEOUT);
 
-
     virtual u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT);
     virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT);
     virtual u_result checkIfTofLidar(bool & isTofLidar, _u32 timeout = DEFAULT_TIMEOUT);
@@ -135,4 +134,4 @@ namespace rp { namespace standalone{ namespace rplidar {
     RPlidarDriverImplCommon();
     virtual ~RPlidarDriverImplCommon() {}
 };
-}}}
\ No newline at end of file
+}}}
diff --git a/src/node.cpp b/src/node.cpp
index 976725f4..59837ba2 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -134,7 +134,6 @@ bool checkRPLIDARHealth(RPlidarDriver * drv)
 {
     u_result     op_result;
     rplidar_response_device_health_t healthinfo;
-
     op_result = drv->getHealth(healthinfo);
     if (IS_OK(op_result)) { 
         ROS_INFO("RPLidar health status : %d", healthinfo.status);
@@ -158,7 +157,6 @@ bool stop_motor(std_srvs::Empty::Request &req,
        return false;
 
   ROS_DEBUG("Stop motor");
-  drv->stop();
   drv->stopMotor();
   return true;
 }
@@ -168,9 +166,14 @@ bool start_motor(std_srvs::Empty::Request &req,
 {
   if(!drv)
        return false;
-  ROS_DEBUG("Start motor");
-  drv->startMotor();
-  drv->startScan(0,1);
+  if(drv->isConnected())
+  {
+      ROS_DEBUG("Start motor");
+      u_result ans=drv->startMotor();
+  
+      ans=drv->startScan(0,1);
+   }
+   else ROS_INFO("lost connection");
   return true;
 }
 
@@ -369,7 +372,6 @@ int main(int argc, char * argv[]) {
                 // All the data is invalid, just publish them
                 float angle_min = DEG2RAD(0.0f);
                 float angle_max = DEG2RAD(359.0f);
-
                 publish_scan(&scan_pub, nodes, count,
                              start_scan_time, scan_duration, inverted,
                              angle_min, angle_max, max_distance,