From 7a355486b31d4d90342dadf5b2af822c91c96881 Mon Sep 17 00:00:00 2001
From: gfuse <majidgeravand68@gmail.com>
Date: Thu, 15 Jul 2021 16:02:11 +0200
Subject: [PATCH] solve the bug in the stop API

---
 .../vio/include/gpu_svo/frame_handler_mono.h  |  2 +-
 GPU_version/vio/launch/Px30.launch            |  4 ++--
 GPU_version/vio/src/depth_filter.cpp          |  1 +
 GPU_version/vio/src/vo_node.cpp               | 19 ++++++++++++++-----
 4 files changed, 18 insertions(+), 8 deletions(-)

diff --git a/GPU_version/vio/include/gpu_svo/frame_handler_mono.h b/GPU_version/vio/include/gpu_svo/frame_handler_mono.h
index c1967c3..2e3b6ed 100644
--- a/GPU_version/vio/include/gpu_svo/frame_handler_mono.h
+++ b/GPU_version/vio/include/gpu_svo/frame_handler_mono.h
@@ -53,7 +53,7 @@ public:
   const vector<cv::Point2f>& initFeatureTrackCurPx() const { return klt_homography_init_->px_cur_; }
 
   /// Access the depth filter.
-  DepthFilter* depthFilter() const { return depth_filter_; }
+  DepthFilter* depthFilter() const{ return depth_filter_; }
 
   /// An external place recognition module may know where to relocalize.
   bool relocalizeFrameAtPose(
diff --git a/GPU_version/vio/launch/Px30.launch b/GPU_version/vio/launch/Px30.launch
index b5371c6..36114e8 100644
--- a/GPU_version/vio/launch/Px30.launch
+++ b/GPU_version/vio/launch/Px30.launch
@@ -1,6 +1,6 @@
 <launch>
 
-  <!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen"/ -->
+  <node pkg="vio" type="vio" name="vio" clear_params="true" output="screen"/>
 
     <!-- Camera topic to subscribe to -->
     <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
@@ -8,7 +8,7 @@
     <param name="vio/cmd_topic" value="/cmd_vel" type="str" />
     <param name="vio/start" value="/vio/start" type="str" />
     <param name="vio/stop" value="/vio/stop" type="str" />
-    <param name="vio/getOdom" value="/vio/set_odom" type="str" />
+    <param name="vio/odom" value="/vio/odom" type="str" />
     
     <!-- Camera calibration file -->
     <rosparam file="$(find vio)/param/px30.yaml" />
diff --git a/GPU_version/vio/src/depth_filter.cpp b/GPU_version/vio/src/depth_filter.cpp
index ba653f2..c7ebf2c 100644
--- a/GPU_version/vio/src/depth_filter.cpp
+++ b/GPU_version/vio/src/depth_filter.cpp
@@ -74,6 +74,7 @@ void DepthFilter::stopThread()
     SVO_INFO_STREAM("DepthFilter interrupt and join thread... ");
     seeds_updating_halt_ = true;
     thread_->interrupt();
+    usleep(5000);
     thread_->join();
     thread_ = NULL;
   }
diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp
index a7df993..a182007 100644
--- a/GPU_version/vio/src/vo_node.cpp
+++ b/GPU_version/vio/src/vo_node.cpp
@@ -32,6 +32,7 @@
 #include <vio/getOdom.h>
 #include <vio/start.h>
 #include <vio/stop.h>
+#include <gpu_svo/depth_filter.h>
 #if VIO_DEBUG
 #include <gpu_svo/visualizer.h>
 #endif
@@ -55,8 +56,11 @@ public:
             start_=true;
             if(vo_->stage()!=svo::FrameHandlerBase::STAGE_DEFAULT_FRAME)
                 vo_->start();
-            else
+            else{
+                vo_->depthFilter()->startThread();
                 vo_->reset();
+            }
+
             imu_the_=new boost::thread(&VioNode::imu_th,this);
             ++trace_id_;
             res.ret=0;
@@ -67,10 +71,15 @@ public:
     };
 
     bool stop(vio::stop::Request& req, vio::stop::Response& res){
-        if(req.off==1){
+        if(req.off==1 && imu_the_!=NULL){
             start_=false;
-            usleep(5000);
-            imu_the_->join();
+            vo_->depthFilter()->stopThread();
+            imu_the_->interrupt();
+            if(imu_the_->get_id()==boost::this_thread::get_id())
+                imu_the_->detach();
+            else
+                imu_the_->join();
+            imu_the_=NULL;
             res.ret=0;
         }else{
             res.ret=100;
@@ -155,7 +164,7 @@ void VioNode::imu_th(){
     ros::NodeHandle nh;
     ros::Subscriber imu_sub=nh.subscribe(vk::getParam<std::string>("vio/imu_topic", "imu/raw"),10,&VioNode::imuCb, this);
     ros::Subscriber cmd_sub=nh.subscribe(vk::getParam<std::string>("vio/cmd_topic", "cmd/raw"),10,&VioNode::cmdCb, this);
-    while(start_)
+    while(start_ && !boost::this_thread::interruption_requested())
     {
         ros::spinOnce();
         usleep(1000);