diff --git a/src/lidar_lite_driver_node.cpp b/src/lidar_lite_driver_node.cpp index cb1c1ead02e646a5e37dd3c4302235074ea8e09c..92b6a28c59146191e8c5cd9b0a0a9044779e10dd 100644 --- a/src/lidar_lite_driver_node.cpp +++ b/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] */