Merge pull request #100 from intel/refactor

update eloquent
intel · Dec 16, 2019 · 47b313c · 47b313c
2 parents 32f19ca + e41d491
commit 47b313c
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 3 deletions.
diff --git a/realsense_ros/include/realsense/rs_base.hpp b/realsense_ros/include/realsense/rs_base.hpp
@@ -75,7 +75,7 @@ class RealSenseBase
   void printSupportedStreamProfiles();
   void printActiveStreamProfiles();
   void printStreamProfiles(const std::vector<rs2::stream_profile> & profile_list);
-
+  void startWorkThread();
 protected:
   Result toggleStream(const stream_index_pair & stream, const rclcpp::Parameter & param);
   Result changeResolution(const stream_index_pair & stream, const rclcpp::Parameter & param);
@@ -100,7 +100,8 @@ class RealSenseBase
   rs2::pipeline pipeline_;
   rs2::config cfg_;
   std::string base_frame_id_;
-
+  rs2::frame_queue frame_data;
+  std::thread work_thread_;
   std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;
   rclcpp::TimerBase::SharedPtr timer_;
   std::map<stream_index_pair, bool> enable_ = {{COLOR, false}, {DEPTH, false},

diff --git a/realsense_ros/src/rs_base.cpp b/realsense_ros/src/rs_base.cpp
@@ -38,6 +38,19 @@ RealSenseBase::RealSenseBase(rs2::context ctx, rs2::device dev, rclcpp::Node & n
 RealSenseBase::~RealSenseBase()
 {
   pipeline_.stop();
+  if (work_thread_.joinable()) {
+      work_thread_.join();
+  }
+}
+
+void RealSenseBase::startWorkThread() 
+{
+  work_thread_ = std::thread([=]() {
+    while (true) {
+      rs2::frame frame = frame_data.wait_for_frame();
+      publishTopicsCallback(frame);
+    }
+  });
 }
 
 void RealSenseBase::startPipeline()
@@ -64,7 +77,9 @@ void RealSenseBase::startPipeline()
     RCLCPP_WARN(node_.get_logger(), "No TF is available. Enable base stream (Depth or Pose) first.");
   }
 
-  pipeline_.start(cfg_, std::bind(&RealSenseBase::publishTopicsCallback, this, std::placeholders::_1));
+  frame_data = rs2::frame_queue(5);
+  pipeline_.start(cfg_, frame_data);
+  startWorkThread();
 }
 
 void RealSenseBase::setupStream(const stream_index_pair & stream)