set fixed frequency

src/lidar_lite_driver_node.cpp

@@ -4,10 +4,10 @@
 
 LidarLiteDriverNode::LidarLiteDriverNode(ros::NodeHandle& nh)
   : iri_base_driver::IriBaseNodeDriver<LidarLiteDriver>(nh)
-  , RunThread_(true)
-  , Thread_(boost::bind(&LidarLiteDriverNode::ThreadFunc, this))
+  // , RunThread_(true)
+  // , Thread_(boost::bind(&LidarLiteDriverNode::ThreadFunc, this))
 {
-  // this->loop_rate_ = 1000; // [Hz]
+  this->loop_rate_ = 30; // [Hz]
 
   // Read from launch file
   std::string serial;
@@ -40,33 +40,54 @@ LidarLiteDriverNode::LidarLiteDriverNode(ros::NodeHandle& nh)
 void
 LidarLiteDriverNode::ThreadFunc(void)
 {
-  while (this->RunThread_) {
-    if (this->driver_.getState() == LidarLiteDriver::RUNNING) {
-      // Get measurement
-      this->driver_.lock();
-      int range = this->driver_.getRange();
-      this->driver_.unlock();
-
-      // Publish ROS message
-      this->range_msg_.header.seq = this->range_seq_;
-      this->range_msg_.header.stamp = ros::Time::now();
-      this->range_msg_.header.frame_id = this->frame_id_;
-      this->range_msg_.field_of_view = 0.1;
-      this->range_msg_.min_range = 0;
-      this->range_msg_.max_range = 40;
-      this->range_msg_.range = (double)range/100.0;
-      this->range_publisher_.publish(this->range_msg_);
-
-      // Increment range publisher counter.
-      ++this->range_seq_;
-    }
-  }
+  // while (this->RunThread_) {
+  //   if (this->driver_.getState() == LidarLiteDriver::RUNNING) {
+  //     // Get measurement
+  //     this->driver_.lock();
+  //     int range = this->driver_.getRange();
+  //     this->driver_.unlock();
+
+  //     // Publish ROS message
+  //     this->range_msg_.header.seq = this->range_seq_;
+  //     this->range_msg_.header.stamp = ros::Time::now();
+  //     this->range_msg_.header.frame_id = this->frame_id_;
+  //     this->range_msg_.field_of_view = 0.1;
+  //     this->range_msg_.min_range = 0;
+  //     this->range_msg_.max_range = 40;
+  //     this->range_msg_.range = (double)range/100.0;
+  //     this->range_publisher_.publish(this->range_msg_);
+
+  //     // Increment range publisher counter.
+  //     ++this->range_seq_;
+  //   }
+  // }
 }
 
 void
 LidarLiteDriverNode::mainNodeThread(void)
 {
   /* code */
+  if (this->driver_.getState() == LidarLiteDriver::RUNNING) {
+
+    // Get measurement
+    this->driver_.lock();
+    int range = this->driver_.getRange();
+    this->driver_.unlock();
+
+    // Publish ROS message
+    this->range_msg_.header.seq = this->range_seq_;
+    this->range_msg_.header.stamp = ros::Time::now();
+    this->range_msg_.header.frame_id = this->frame_id_;
+    this->range_msg_.field_of_view = 0.1;
+    this->range_msg_.min_range = 0;
+    this->range_msg_.max_range = 40;
+    this->range_msg_.range = (double)range/100.0;
+    this->range_publisher_.publish(this->range_msg_);
+
+    // Increment range publisher counter.
+    ++this->range_seq_;
+  }
+
 }
 
 /*  [subscriber callbacks] */