From ab6f08f229f89c3054ce53ae461b0c743936f570 Mon Sep 17 00:00:00 2001
From: Angel Santamaria-Navarro <asantamaria@iri.upc.edu>
Date: Tue, 29 Nov 2016 20:40:03 +0100
Subject: [PATCH] Driver version
---
CMakeLists.txt | 2 +-
README.md | 8 +--
include/lidar_lite_driver_node.h | 6 +++
src/lidar_lite_driver.cpp | 24 +++++----
src/lidar_lite_driver_node.cpp | 90 ++++++++++++++------------------
5 files changed, 64 insertions(+), 66 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ab98106..bb9625a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -9,7 +9,7 @@ find_package(catkin REQUIRED)
find_package(catkin REQUIRED COMPONENTS iri_base_driver sensor_msgs)
## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
+find_package(Boost REQUIRED COMPONENTS thread)
# ********************************************************************
# Add system and labrobotica dependencies here
diff --git a/README.md b/README.md
index 51cf409..265c1ab 100644
--- a/README.md
+++ b/README.md
@@ -9,22 +9,22 @@ ROS wrapper to work with [LidarLite](https://gitlab.iri.upc.edu/asantamaria/Lida
- [iriutils](https://devel.iri.upc.edu/pub/labrobotica/drivers/iriutils/trunk) - IRI C++ library
- Download the library: `svn co https://devel.iri.upc.edu/pub/labrobotica/drivers/iriutils/trunk comm`
- - Compile and install: `cd iriutils/build && cmake .. && make && sudo make install`
+ - Compile and install: `cd iriutils/build && cmake -D CMAKE_BUILD_TYPE=RELEASE .. && make && sudo make install`
- [comm](https://devel.iri.upc.edu/pub/labrobotica/drivers/comm/trunk) - IRI C++ library
- Download the library: `svn co https://devel.iri.upc.edu/pub/labrobotica/drivers/comm/trunk comm`
- - Compile and install: `cd comm/build && cmake .. && make && sudo make install`
+ - Compile and install: `cd comm/build && cmake -D CMAKE_BUILD_TYPE=RELEASE .. && make && sudo make install`
- [usb_i2c_adapter](https://devel.iri.upc.edu/pub/labrobotica/drivers/usb_i2c_adapter/trunk) - IRI C++ library
- Download the library: `svn co https://devel.iri.upc.edu/pub/labrobotica/drivers/usb_i2c_adapter/trunk usb_i2c_adapter`
- - Compile and install: `cd usb_i2c_adapter/build && cmake .. && make && sudo make install`
+ - Compile and install: `cd usb_i2c_adapter/build && cmake -D CMAKE_BUILD_TYPE=RELEASE .. && make && sudo make install`
- [LidarLite](https://gitlab.iri.upc.edu/asantamaria/LidarLite) - IRI C++ library
- Download the library: `git clone https://gitlab.iri.upc.edu/asantamaria/LidarLite.git`
- - Compile and install: `cd LidarLite/build && cmake .. && make && sudo make install`
+ - Compile and install: `cd LidarLite/build && cmake -D CMAKE_BUILD_TYPE=RELEASE .. && make && sudo make install`
##### ROS dependencies
diff --git a/include/lidar_lite_driver_node.h b/include/lidar_lite_driver_node.h
index a6adc93..aacf55d 100644
--- a/include/lidar_lite_driver_node.h
+++ b/include/lidar_lite_driver_node.h
@@ -28,6 +28,8 @@
#include <iri_base_driver/iri_base_driver_node.h>
#include "lidar_lite_driver.h"
+#include <boost/thread/thread.hpp>
+
// [publisher subscriber headers]
#include <sensor_msgs/Range.h>
@@ -59,6 +61,10 @@ class LidarLiteDriverNode : public iri_base_driver::IriBaseNodeDriver<LidarLiteD
int range_seq_; // Range publisher counter.
std::string frame_id_; // publisher frame id.
+ boost::thread Thread_; // Hardware reading thread.
+ bool RunThread_; // Enables the hardware reading thread.
+ void ThreadFunc(void); // Hardware reading thread function.
+
// [publisher attributes]
ros::Publisher range_publisher_;
sensor_msgs::Range range_msg_;
diff --git a/src/lidar_lite_driver.cpp b/src/lidar_lite_driver.cpp
index 848158c..7296454 100644
--- a/src/lidar_lite_driver.cpp
+++ b/src/lidar_lite_driver.cpp
@@ -9,6 +9,17 @@ LidarLiteDriver::LidarLiteDriver(void)
bool LidarLiteDriver::openDriver(void)
{
+ this->laser_ptr_ = new CLidarLite(this->serial_,this->config_mode_);
+
+ try
+ {
+ this->laser_ptr_->open();
+ }
+ catch(CLidarLiteException &e)
+ {
+ std::cout << e.what() << std::endl;
+ }
+
return true;
}
@@ -17,7 +28,8 @@ bool LidarLiteDriver::closeDriver(void)
try
{
this->laser_ptr_->close();
- }catch(CLidarLiteException &e)
+ }
+ catch(CLidarLiteException &e)
{
std::cout << e.what() << std::endl;
}
@@ -33,16 +45,6 @@ bool LidarLiteDriver::closeDriver(void)
bool LidarLiteDriver::startDriver(void)
{
- this->laser_ptr_ = new CLidarLite(this->serial_,this->config_mode_);
-
- try
- {
- this->laser_ptr_->open();
- }catch(CLidarLiteException &e)
- {
- std::cout << e.what() << std::endl;
- }
-
return true;
}
diff --git a/src/lidar_lite_driver_node.cpp b/src/lidar_lite_driver_node.cpp
index 02a1b45..552b613 100644
--- a/src/lidar_lite_driver_node.cpp
+++ b/src/lidar_lite_driver_node.cpp
@@ -1,38 +1,27 @@
#include "lidar_lite_driver_node.h"
+#include <ctime>
+
+
LidarLiteDriverNode::LidarLiteDriverNode(ros::NodeHandle &nh) :
- iri_base_driver::IriBaseNodeDriver<LidarLiteDriver>(nh)
+ iri_base_driver::IriBaseNodeDriver<LidarLiteDriver>(nh),
+ RunThread_(true),
+ Thread_(boost::bind(&LidarLiteDriverNode::ThreadFunc, this))
{
+ // this->loop_rate_ = 1000; // [Hz]
+
// Read from launch file
std::string serial;
- this->public_node_handle_.param<std::string>("serial_num", serial, "A700evSl");
-
int config_mode;
+ this->public_node_handle_.param<std::string>("serial_num", serial, "A700evSl");
this->public_node_handle_.param<int>("config_mode", config_mode, 0);
-
- int freq;
- switch (config_mode)
- {
- default:
- freq = 270;
- break;
- case 1:
- freq = 1000;
- break;
- case 2:
- freq = 650;
- break;
- }
- this->loop_rate_ = freq;
-
this->public_node_handle_.param<std::string>("frame_id", this->frame_id_, "lidar_lite");
-
// Set device parameters
this->driver_.setParams(serial,config_mode);
// [init publishers]
- this->range_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Range>("range", 1);
+ this->range_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Range>("range", 100);
this->range_seq_ = 0;
// [init subscribers]
@@ -46,37 +35,36 @@ LidarLiteDriverNode::LidarLiteDriverNode(ros::NodeHandle &nh) :
// [init action clients]
}
-void LidarLiteDriverNode::mainNodeThread(void)
+void LidarLiteDriverNode::ThreadFunc(void)
{
- //lock access to driver if necessary
- this->driver_.lock();
-
- // Get measurement
- int range = this->driver_.getRange();
-
- // [fill msg Header if necessary]
-
- // [fill msg structures]
- 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 = range;
-
- // [fill srv structure and make request to the server]
-
- // [fill action structure and make request to the action server]
-
- // [publish messages]
- 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 = range;
+ this->range_publisher_.publish(this->range_msg_);
+
+ // Increment range publisher counter.
+ ++this->range_seq_;
+ }
+ }
+}
- //unlock access to driver if previously blocked
- this->driver_.unlock();
+void LidarLiteDriverNode::mainNodeThread(void)
+{
+ /* code */
}
/* [subscriber callbacks] */
@@ -113,6 +101,8 @@ void LidarLiteDriverNode::reconfigureNodeHook(int level)
LidarLiteDriverNode::~LidarLiteDriverNode(void)
{
+ this->RunThread_ = false;
+ this->Thread_.join();
// [free dynamic memory]
}
--
GitLab