public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

solve the bug in the stop API

Browse Source
This commit is contained in:
gfuse
2021-07-15 16:02:11 +02:00
parent 3b17b7c0f9
commit 7a355486b3
4 changed files with 18 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/launch/Px30.launch
+2 -2
View File
@@ -1,6 +1,6 @@
<launch> <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 --> <!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" /> <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/cmd_topic" value="/cmd_vel" type="str" />
<param name="vio/start" value="/vio/start" type="str" /> <param name="vio/start" value="/vio/start" type="str" />
<param name="vio/stop" value="/vio/stop" 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 --> <!-- Camera calibration file -->
<rosparam file="$(find vio)/param/px30.yaml" /> <rosparam file="$(find vio)/param/px30.yaml" />
GPU_version/vio/src/depth_filter.cpp
+1
View File
@@ -74,6 +74,7 @@ void DepthFilter::stopThread()
SVO_INFO_STREAM("DepthFilter interrupt and join thread... "); SVO_INFO_STREAM("DepthFilter interrupt and join thread... ");
seeds_updating_halt_ = true; seeds_updating_halt_ = true;
thread_->interrupt(); thread_->interrupt();
usleep(5000);
thread_->join(); thread_->join();
thread_ = NULL; thread_ = NULL;
} }
GPU_version/vio/src/vo_node.cpp
+13 -4
View File
@@ -32,6 +32,7 @@
#include <vio/getOdom.h> #include <vio/getOdom.h>
#include <vio/start.h> #include <vio/start.h>
#include <vio/stop.h> #include <vio/stop.h>
#include <gpu_svo/depth_filter.h>
#if VIO_DEBUG #if VIO_DEBUG
#include <gpu_svo/visualizer.h> #include <gpu_svo/visualizer.h>
#endif #endif
@@ -55,8 +56,11 @@ public:
start_=true; start_=true;
if(vo_->stage()!=svo::FrameHandlerBase::STAGE_DEFAULT_FRAME) if(vo_->stage()!=svo::FrameHandlerBase::STAGE_DEFAULT_FRAME)
vo_->start(); vo_->start();
else else{
vo_->depthFilter()->startThread();
vo_->reset(); vo_->reset();
}
imu_the_=new boost::thread(&VioNode::imu_th,this); imu_the_=new boost::thread(&VioNode::imu_th,this);
++trace_id_; ++trace_id_;
res.ret=0; res.ret=0;
@@ -67,10 +71,15 @@ public:
}; };
bool stop(vio::stop::Request& req, vio::stop::Response& res){ bool stop(vio::stop::Request& req, vio::stop::Response& res){
if(req.off==1){ if(req.off==1 && imu_the_!=NULL){
start_=false; start_=false;
usleep(5000); 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_->join();
imu_the_=NULL;
res.ret=0; res.ret=0;
}else{ }else{
res.ret=100; res.ret=100;
@@ -155,7 +164,7 @@ void VioNode::imu_th(){
ros::NodeHandle nh; ros::NodeHandle nh;
ros::Subscriber imu_sub=nh.subscribe(vk::getParam<std::string>("vio/imu_topic", "imu/raw"),10,&VioNode::imuCb, this); 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); 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(); ros::spinOnce();
usleep(1000); usleep(1000);