From 082c918ce779eaa7b47e7a14bab404b7874b7377 Mon Sep 17 00:00:00 2001
From: gfuse <majidgeravand68@gmail.com>
Date: Thu, 2 Sep 2021 14:52:26 +0200
Subject: [PATCH] optimized cpu usage

---
 GPU_version/vio/launch/Px30.launch |  2 +-
 GPU_version/vio/src/vo_node.cpp    | 13 +++++++++----
 2 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/GPU_version/vio/launch/Px30.launch b/GPU_version/vio/launch/Px30.launch
index 19ad5f0..d12f08d 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" launch-prefix="xterm -e gdb args" / -->
+  <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" />
diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp
index 7f0bbd4..c97c179 100644
--- a/GPU_version/vio/src/vo_node.cpp
+++ b/GPU_version/vio/src/vo_node.cpp
@@ -33,6 +33,7 @@
 #include <vio/start.h>
 #include <vio/stop.h>
 #include <vio/depth_filter.h>
+#include <ros/callback_queue.h>
 #if VIO_DEBUG
 #include <vio/visualizer.h>
 #endif
@@ -169,11 +170,13 @@ void VioNode::cmdCb(const geometry_msgs::TwistPtr &cmd) {
 }
 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);
+    ros::CallbackQueue Q;
+    nh.setCallbackQueue(&Q);
+    ros::Subscriber imu_sub=nh.subscribe(vk::getParam<std::string>("vio/imu_topic", "imu/raw"),10,&VioNode::imuCb, this,ros::TransportHints().tcpNoDelay());
+    ros::Subscriber cmd_sub=nh.subscribe(vk::getParam<std::string>("vio/cmd_topic", "cmd/raw"),10,&VioNode::cmdCb, this,ros::TransportHints().tcpNoDelay());
     while(start_ && !boost::this_thread::interruption_requested())
     {
-        ros::spinOnce();
+        Q.callOne(ros::WallDuration(10,0.0));
         usleep(1000);
     }
 
@@ -198,6 +201,8 @@ int main(int argc, char **argv)
 {
   ros::init(argc, argv, "vio");
   ros::NodeHandle nh;
+  ros::CallbackQueue Q;
+  nh.setCallbackQueue(&Q);
   VioNode vio;
   ros::ServiceServer start = nh.advertiseService(vk::getParam<std::string>("vio/start", "start"),
                                                  &VioNode::start, &vio);
@@ -210,7 +215,7 @@ int main(int argc, char **argv)
   // start processing callbacks
   while(ros::ok())
   {
-      ros::spinOnce();
+      Q.callOne(ros::WallDuration(30,0.0));
   }
   printf("SVO terminated.\n");
   return 0;