Skip to content
Snippets Groups Projects
Commit 328e696a authored by Angel Santamaria-Navarro's avatar Angel Santamaria-Navarro
Browse files

Clang-format style: Mozilla

parent ab6f08f2
No related branches found
No related tags found
No related merge requests found
// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC. // Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial,
// Author Angel Santamaria-Navarro // CSIC-UPC.
// Author Angel Santamaria-Navarro
// All rights reserved. // All rights reserved.
// //
// This file is part of iri-ros-pkg // This file is part of iri-ros-pkg
...@@ -15,9 +16,10 @@ ...@@ -15,9 +16,10 @@
// //
// You should have received a copy of the GNU Lesser General Public License // You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
// //
// IMPORTANT NOTE: This code has been generated through a script from the // IMPORTANT NOTE: This code has been generated through a script from the
// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness // iri_ros_scripts. Please do NOT delete any comments to guarantee the
// correctness
// of the scripts. ROS topics can be easly add by using those scripts. Please // of the scripts. ROS topics can be easly add by using those scripts. Please
// refer to the IRI wiki page for more information: // refer to the IRI wiki page for more information:
// http://wikiri.upc.es/index.php/Robotics_Lab // http://wikiri.upc.es/index.php/Robotics_Lab
...@@ -35,133 +37,136 @@ ...@@ -35,133 +37,136 @@
* \brief IRI ROS Specific Driver Class * \brief IRI ROS Specific Driver Class
* *
* This class inherits from the IRI Base class IriBaseDriver, which provides the * This class inherits from the IRI Base class IriBaseDriver, which provides the
* guidelines to implement any specific driver. The IriBaseDriver class offers an * guidelines to implement any specific driver. The IriBaseDriver class offers
* an
* easy framework to integrate functional drivers implemented in C++ with the * easy framework to integrate functional drivers implemented in C++ with the
* ROS driver structure. ROS driver_base state transitions are already managed * ROS driver structure. ROS driver_base state transitions are already managed
* by IriBaseDriver. * by IriBaseDriver.
* *
* The LidarLiteDriver class must implement all specific driver requirements to * The LidarLiteDriver class must implement all specific driver requirements to
* safetely open, close, run and stop the driver at any time. It also must * safetely open, close, run and stop the driver at any time. It also must
* guarantee an accessible interface for all driver's parameters. * guarantee an accessible interface for all driver's parameters.
* *
* The LidarLiteConfig.cfg needs to be filled up with those parameters suitable * The LidarLiteConfig.cfg needs to be filled up with those parameters suitable
* to be changed dynamically by the ROS dyanmic reconfigure application. The * to be changed dynamically by the ROS dyanmic reconfigure application. The
* implementation of the CIriNode class will manage those parameters through * implementation of the CIriNode class will manage those parameters through
* methods like postNodeOpenHook() and reconfigureNodeHook(). * methods like postNodeOpenHook() and reconfigureNodeHook().
* *
*/ */
class LidarLiteDriver : public iri_base_driver::IriBaseDriver class LidarLiteDriver : public iri_base_driver::IriBaseDriver
{ {
private: private:
// private attributes and methods // private attributes and methods
CLidarLite* laser_ptr_; CLidarLite* laser_ptr_;
bool params_set_; bool params_set_;
std::string serial_; std::string serial_;
int config_mode_; int config_mode_;
public: public:
/** /**
* \brief define config type * \brief define config type
* *
* Define a Config type with the LidarLiteConfig. All driver implementations * Define a Config type with the LidarLiteConfig. All driver implementations
* will then use the same variable type Config. * will then use the same variable type Config.
*/ */
typedef iri_lidar_lite::LidarLiteConfig Config; typedef iri_lidar_lite::LidarLiteConfig Config;
/** /**
* \brief config variable * \brief config variable
* *
* This variable has all the driver parameters defined in the cfg config file. * This variable has all the driver parameters defined in the cfg config file.
* Is updated everytime function config_update() is called. * Is updated everytime function config_update() is called.
*/ */
Config config_; Config config_;
/** /**
* \brief constructor * \brief constructor
* *
* In this constructor parameters related to the specific driver can be * In this constructor parameters related to the specific driver can be
* initalized. Those parameters can be also set in the openDriver() function. * initalized. Those parameters can be also set in the openDriver() function.
* Attributes from the main node driver class IriBaseDriver such as loop_rate, * Attributes from the main node driver class IriBaseDriver such as loop_rate,
* may be also overload here. * may be also overload here.
*/ */
LidarLiteDriver(void); LidarLiteDriver(void);
/** /**
* \brief open driver * \brief open driver
* *
* In this function, the driver must be openned. Openning errors must be * In this function, the driver must be openned. Openning errors must be
* taken into account. This function is automatically called by * taken into account. This function is automatically called by
* IriBaseDriver::doOpen(), an state transition is performed if return value * IriBaseDriver::doOpen(), an state transition is performed if return value
* equals true. * equals true.
* *
* \return bool successful * \return bool successful
*/ */
bool openDriver(void); bool openDriver(void);
/** /**
* \brief close driver * \brief close driver
* *
* In this function, the driver must be closed. Variables related to the * In this function, the driver must be closed. Variables related to the
* driver state must also be taken into account. This function is automatically * driver state must also be taken into account. This function is
* called by IriBaseDriver::doClose(), an state transition is performed if * automatically
* return value equals true. * called by IriBaseDriver::doClose(), an state transition is performed if
* * return value equals true.
* \return bool successful *
*/ * \return bool successful
bool closeDriver(void); */
bool closeDriver(void);
/**
* \brief start driver /**
* * \brief start driver
* After this function, the driver and its thread will be started. The driver *
* and related variables should be properly setup. This function is * After this function, the driver and its thread will be started. The driver
* automatically called by IriBaseDriver::doStart(), an state transition is * and related variables should be properly setup. This function is
* performed if return value equals true. * automatically called by IriBaseDriver::doStart(), an state transition is
* * performed if return value equals true.
* \return bool successful *
*/ * \return bool successful
bool startDriver(void); */
bool startDriver(void);
/**
* \brief stop driver /**
* * \brief stop driver
* After this function, the driver's thread will stop its execution. The driver *
* and related variables should be properly setup. This function is * After this function, the driver's thread will stop its execution. The
* automatically called by IriBaseDriver::doStop(), an state transition is * driver
* performed if return value equals true. * and related variables should be properly setup. This function is
* * automatically called by IriBaseDriver::doStop(), an state transition is
* \return bool successful * performed if return value equals true.
*/ *
bool stopDriver(void); * \return bool successful
*/
/** bool stopDriver(void);
* \brief config update
* /**
* In this function the driver parameters must be updated with the input * \brief config update
* config variable. Then the new configuration state will be stored in the *
* Config attribute. * In this function the driver parameters must be updated with the input
* * config variable. Then the new configuration state will be stored in the
* \param new_cfg the new driver configuration state * Config attribute.
* *
* \param level level in which the update is taken place * \param new_cfg the new driver configuration state
*/ *
void config_update(Config& new_cfg, uint32_t level=0); * \param level level in which the update is taken place
*/
// here define all lidar_lite_driver interface methods to retrieve and set void config_update(Config& new_cfg, uint32_t level = 0);
// the driver parameters
// here define all lidar_lite_driver interface methods to retrieve and set
void setParams(const std::string &serial, const int &config_mode); // the driver parameters
int getRange(void); void setParams(const std::string& serial, const int& config_mode);
/** int getRange(void);
* \brief Destructor
* /**
* This destructor is called when the object is about to be destroyed. * \brief Destructor
* *
*/ * This destructor is called when the object is about to be destroyed.
~LidarLiteDriver(void); *
*/
~LidarLiteDriver(void);
}; };
#endif #endif
// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC. // Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial,
// CSIC-UPC.
// Author Angel Santamaria-Navarro // Author Angel Santamaria-Navarro
// All rights reserved. // All rights reserved.
// //
...@@ -15,9 +16,10 @@ ...@@ -15,9 +16,10 @@
// //
// You should have received a copy of the GNU Lesser General Public License // You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
// //
// IMPORTANT NOTE: This code has been generated through a script from the // IMPORTANT NOTE: This code has been generated through a script from the
// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness // iri_ros_scripts. Please do NOT delete any comments to guarantee the
// correctness
// of the scripts. ROS topics can be easly add by using those scripts. Please // of the scripts. ROS topics can be easly add by using those scripts. Please
// refer to the IRI wiki page for more information: // refer to the IRI wiki page for more information:
// http://wikiri.upc.es/index.php/Robotics_Lab // http://wikiri.upc.es/index.php/Robotics_Lab
...@@ -25,8 +27,8 @@ ...@@ -25,8 +27,8 @@
#ifndef _lidar_lite_driver_node_h_ #ifndef _lidar_lite_driver_node_h_
#define _lidar_lite_driver_node_h_ #define _lidar_lite_driver_node_h_
#include <iri_base_driver/iri_base_driver_node.h>
#include "lidar_lite_driver.h" #include "lidar_lite_driver.h"
#include <iri_base_driver/iri_base_driver_node.h>
#include <boost/thread/thread.hpp> #include <boost/thread/thread.hpp>
...@@ -40,155 +42,162 @@ ...@@ -40,155 +42,162 @@
/** /**
* \brief IRI ROS Specific Driver Class * \brief IRI ROS Specific Driver Class
* *
* This class inherits from the IRI Core class IriBaseNodeDriver<IriBaseDriver>, * This class inherits from the IRI Core class IriBaseNodeDriver<IriBaseDriver>,
* to provide an execution thread to the driver object. A complete framework * to provide an execution thread to the driver object. A complete framework
* with utilites to test the node functionallity or to add diagnostics to * with utilites to test the node functionallity or to add diagnostics to
* specific situations is also given. The inherit template design form allows * specific situations is also given. The inherit template design form allows
* complete access to any IriBaseDriver object implementation. * complete access to any IriBaseDriver object implementation.
* *
* As mentioned, tests in the different driver states can be performed through * As mentioned, tests in the different driver states can be performed through
* class methods such as addNodeOpenedTests() or addNodeRunningTests(). Tests * class methods such as addNodeOpenedTests() or addNodeRunningTests(). Tests
* common to all nodes may be also executed in the pattern class IriBaseNodeDriver. * common to all nodes may be also executed in the pattern class
* IriBaseNodeDriver.
* Similarly to the tests, diagnostics can easyly be added. See ROS Wiki for * Similarly to the tests, diagnostics can easyly be added. See ROS Wiki for
* more details: * more details:
* http://www.ros.org/wiki/diagnostics/ (Tutorials: Creating a Diagnostic Analyzer) * http://www.ros.org/wiki/diagnostics/ (Tutorials: Creating a Diagnostic
* Analyzer)
* http://www.ros.org/wiki/self_test/ (Example: Self Test) * http://www.ros.org/wiki/self_test/ (Example: Self Test)
*/ */
class LidarLiteDriverNode : public iri_base_driver::IriBaseNodeDriver<LidarLiteDriver> class LidarLiteDriverNode
: public iri_base_driver::IriBaseNodeDriver<LidarLiteDriver>
{ {
private: private:
int range_seq_; // Range publisher counter.
int range_seq_; // Range publisher counter. std::string frame_id_; // publisher frame id.
std::string frame_id_; // publisher frame id.
boost::thread Thread_; // Hardware reading thread.
boost::thread Thread_; // Hardware reading thread. bool RunThread_; // Enables the hardware reading thread.
bool RunThread_; // Enables the hardware reading thread. void ThreadFunc(void); // Hardware reading thread function.
void ThreadFunc(void); // Hardware reading thread function.
// [publisher attributes]
// [publisher attributes] ros::Publisher range_publisher_;
ros::Publisher range_publisher_; sensor_msgs::Range range_msg_;
sensor_msgs::Range range_msg_;
// [subscriber attributes]
// [subscriber attributes]
// [service attributes]
// [service attributes]
// [client attributes]
// [client attributes]
// [action server attributes]
// [action server attributes]
// [action client attributes]
// [action client attributes]
/**
/** * \brief post open hook
* \brief post open hook *
* * This function is called by IriBaseNodeDriver::postOpenHook(). In this
* This function is called by IriBaseNodeDriver::postOpenHook(). In this function * function
* specific parameters from the driver must be added so the ROS dynamic * specific parameters from the driver must be added so the ROS dynamic
* reconfigure application can update them. * reconfigure application can update them.
*/ */
void postNodeOpenHook(void); void postNodeOpenHook(void);
public: public:
/** /**
* \brief constructor * \brief constructor
* *
* This constructor mainly creates and initializes the LidarLiteDriverNode topics * This constructor mainly creates and initializes the LidarLiteDriverNode
* through the given public_node_handle object. IriBaseNodeDriver attributes * topics
* may be also modified to suit node specifications. * through the given public_node_handle object. IriBaseNodeDriver attributes
* * may be also modified to suit node specifications.
* All kind of ROS topics (publishers, subscribers, servers or clients) can *
* be easyly generated with the scripts in the iri_ros_scripts package. Refer * All kind of ROS topics (publishers, subscribers, servers or clients) can
* to ROS and IRI Wiki pages for more details: * be easyly generated with the scripts in the iri_ros_scripts package. Refer
* * to ROS and IRI Wiki pages for more details:
* http://www.ros.org/wiki/ROS/Tutorials/WritingPublisherSubscriber(c++) *
* http://www.ros.org/wiki/ROS/Tutorials/WritingServiceClient(c++) * http://www.ros.org/wiki/ROS/Tutorials/WritingPublisherSubscriber(c++)
* http://wikiri.upc.es/index.php/Robotics_Lab * http://www.ros.org/wiki/ROS/Tutorials/WritingServiceClient(c++)
* * http://wikiri.upc.es/index.php/Robotics_Lab
* \param nh a reference to the node handle object to manage all ROS topics. *
*/ * \param nh a reference to the node handle object to manage all ROS topics.
LidarLiteDriverNode(ros::NodeHandle& nh); */
LidarLiteDriverNode(ros::NodeHandle& nh);
/**
* \brief Destructor /**
* * \brief Destructor
* This destructor is called when the object is about to be destroyed. *
* * This destructor is called when the object is about to be destroyed.
*/ *
~LidarLiteDriverNode(void); */
~LidarLiteDriverNode(void);
protected:
/** protected:
* \brief main node thread /**
* * \brief main node thread
* This is the main thread node function. Code written here will be executed *
* in every node loop while the driver is on running state. Loop frequency * This is the main thread node function. Code written here will be executed
* can be tuned by modifying loop_rate attribute. * in every node loop while the driver is on running state. Loop frequency
* * can be tuned by modifying loop_rate attribute.
* Here data related to the process loop or to ROS topics (mainly data structs *
* related to the MSG and SRV files) must be updated. ROS publisher objects * Here data related to the process loop or to ROS topics (mainly data structs
* must publish their data in this process. ROS client servers may also * related to the MSG and SRV files) must be updated. ROS publisher objects
* request data to the corresponding server topics. * must publish their data in this process. ROS client servers may also
*/ * request data to the corresponding server topics.
void mainNodeThread(void); */
void mainNodeThread(void);
// [diagnostic functions]
// [diagnostic functions]
/**
* \brief node add diagnostics /**
* * \brief node add diagnostics
* In this function ROS diagnostics applied to this specific node may be *
* added. Common use diagnostics for all nodes are already called from * In this function ROS diagnostics applied to this specific node may be
* IriBaseNodeDriver::addDiagnostics(), which also calls this function. Information * added. Common use diagnostics for all nodes are already called from
* of how ROS diagnostics work can be readen here: * IriBaseNodeDriver::addDiagnostics(), which also calls this function.
* http://www.ros.org/wiki/diagnostics/ * Information
* http://www.ros.org/doc/api/diagnostic_updater/html/example_8cpp-source.html * of how ROS diagnostics work can be readen here:
*/ * http://www.ros.org/wiki/diagnostics/
void addNodeDiagnostics(void); * http://www.ros.org/doc/api/diagnostic_updater/html/example_8cpp-source.html
*/
// [driver test functions] void addNodeDiagnostics(void);
/** // [driver test functions]
* \brief open status driver tests
* /**
* In this function tests checking driver's functionallity when driver_base * \brief open status driver tests
* status=open can be added. Common use tests for all nodes are already called *
* from IriBaseNodeDriver tests methods. For more details on how ROS tests work, * In this function tests checking driver's functionallity when driver_base
* please refer to the Self Test example in: * status=open can be added. Common use tests for all nodes are already called
* http://www.ros.org/wiki/self_test/ * from IriBaseNodeDriver tests methods. For more details on how ROS tests
*/ * work,
void addNodeOpenedTests(void); * please refer to the Self Test example in:
* http://www.ros.org/wiki/self_test/
/** */
* \brief stop status driver tests void addNodeOpenedTests(void);
*
* In this function tests checking driver's functionallity when driver_base /**
* status=stop can be added. Common use tests for all nodes are already called * \brief stop status driver tests
* from IriBaseNodeDriver tests methods. For more details on how ROS tests work, *
* please refer to the Self Test example in: * In this function tests checking driver's functionallity when driver_base
* http://www.ros.org/wiki/self_test/ * status=stop can be added. Common use tests for all nodes are already called
*/ * from IriBaseNodeDriver tests methods. For more details on how ROS tests
void addNodeStoppedTests(void); * work,
* please refer to the Self Test example in:
/** * http://www.ros.org/wiki/self_test/
* \brief run status driver tests */
* void addNodeStoppedTests(void);
* In this function tests checking driver's functionallity when driver_base
* status=run can be added. Common use tests for all nodes are already called /**
* from IriBaseNodeDriver tests methods. For more details on how ROS tests work, * \brief run status driver tests
* please refer to the Self Test example in: *
* http://www.ros.org/wiki/self_test/ * In this function tests checking driver's functionallity when driver_base
*/ * status=run can be added. Common use tests for all nodes are already called
void addNodeRunningTests(void); * from IriBaseNodeDriver tests methods. For more details on how ROS tests
* work,
/** * please refer to the Self Test example in:
* \brief specific node dynamic reconfigure * http://www.ros.org/wiki/self_test/
* */
* This function is called reconfigureHook() void addNodeRunningTests(void);
*
* \param level integer /**
*/ * \brief specific node dynamic reconfigure
void reconfigureNodeHook(int level); *
* This function is called reconfigureHook()
*
* \param level integer
*/
void reconfigureNodeHook(int level);
}; };
#endif #endif
...@@ -7,60 +7,57 @@ LidarLiteDriver::LidarLiteDriver(void) ...@@ -7,60 +7,57 @@ LidarLiteDriver::LidarLiteDriver(void)
this->config_mode_ = 0; this->config_mode_ = 0;
} }
bool LidarLiteDriver::openDriver(void) bool
LidarLiteDriver::openDriver(void)
{ {
this->laser_ptr_ = new CLidarLite(this->serial_,this->config_mode_); this->laser_ptr_ = new CLidarLite(this->serial_, this->config_mode_);
try try {
{
this->laser_ptr_->open(); this->laser_ptr_->open();
} } catch (CLidarLiteException& e) {
catch(CLidarLiteException &e)
{
std::cout << e.what() << std::endl; std::cout << e.what() << std::endl;
} }
return true; return true;
} }
bool LidarLiteDriver::closeDriver(void) bool
LidarLiteDriver::closeDriver(void)
{ {
try try {
{
this->laser_ptr_->close(); this->laser_ptr_->close();
} } catch (CLidarLiteException& e) {
catch(CLidarLiteException &e)
{
std::cout << e.what() << std::endl; std::cout << e.what() << std::endl;
} }
if(this->laser_ptr_ != NULL) if (this->laser_ptr_ != NULL) {
{
this->laser_ptr_ = NULL; this->laser_ptr_ = NULL;
delete this->laser_ptr_; delete this->laser_ptr_;
} }
return true; return true;
} }
bool LidarLiteDriver::startDriver(void) bool
LidarLiteDriver::startDriver(void)
{ {
return true; return true;
} }
bool LidarLiteDriver::stopDriver(void) bool
LidarLiteDriver::stopDriver(void)
{ {
return true; return true;
} }
void LidarLiteDriver::config_update(Config& new_cfg, uint32_t level) void
LidarLiteDriver::config_update(Config& new_cfg, uint32_t level)
{ {
this->lock(); this->lock();
// depending on current state // depending on current state
// update driver with new_cfg data // update driver with new_cfg data
switch(this->getState()) switch (this->getState()) {
{
case LidarLiteDriver::CLOSED: case LidarLiteDriver::CLOSED:
break; break;
...@@ -71,12 +68,9 @@ void LidarLiteDriver::config_update(Config& new_cfg, uint32_t level) ...@@ -71,12 +68,9 @@ void LidarLiteDriver::config_update(Config& new_cfg, uint32_t level)
this->config_mode_ = new_cfg.Config_mode; this->config_mode_ = new_cfg.Config_mode;
try try {
{
this->laser_ptr_->set_new_config(this->config_mode_); this->laser_ptr_->set_new_config(this->config_mode_);
} } catch (CLidarLiteException& e) {
catch(CLidarLiteException &e)
{
std::cout << e.what() << std::endl; std::cout << e.what() << std::endl;
} }
...@@ -84,27 +78,26 @@ void LidarLiteDriver::config_update(Config& new_cfg, uint32_t level) ...@@ -84,27 +78,26 @@ void LidarLiteDriver::config_update(Config& new_cfg, uint32_t level)
} }
// save the current configuration // save the current configuration
this->config_=new_cfg; this->config_ = new_cfg;
this->unlock(); this->unlock();
} }
void LidarLiteDriver::setParams(const std::string &serial, const int &config_mode) void
LidarLiteDriver::setParams(const std::string& serial, const int& config_mode)
{ {
this->serial_ = serial; this->serial_ = serial;
this->config_mode_ = config_mode; this->config_mode_ = config_mode;
} }
int LidarLiteDriver::getRange(void) int
LidarLiteDriver::getRange(void)
{ {
int range; int range;
try try {
{
range = this->laser_ptr_->get_range(true); range = this->laser_ptr_->get_range(true);
} } catch (CLidarLiteException& e) {
catch(CLidarLiteException &e)
{
std::cout << e.what() << std::endl; std::cout << e.what() << std::endl;
} }
...@@ -113,9 +106,8 @@ int LidarLiteDriver::getRange(void) ...@@ -113,9 +106,8 @@ int LidarLiteDriver::getRange(void)
LidarLiteDriver::~LidarLiteDriver(void) LidarLiteDriver::~LidarLiteDriver(void)
{ {
if(this->laser_ptr_ != NULL) if (this->laser_ptr_ != NULL) {
{
this->laser_ptr_ = NULL; this->laser_ptr_ = NULL;
delete this->laser_ptr_; delete this->laser_ptr_;
} }
} }
...@@ -2,67 +2,69 @@ ...@@ -2,67 +2,69 @@
#include <ctime> #include <ctime>
LidarLiteDriverNode::LidarLiteDriverNode(ros::NodeHandle& nh)
LidarLiteDriverNode::LidarLiteDriverNode(ros::NodeHandle &nh) : : iri_base_driver::IriBaseNodeDriver<LidarLiteDriver>(nh)
iri_base_driver::IriBaseNodeDriver<LidarLiteDriver>(nh), , RunThread_(true)
RunThread_(true), , Thread_(boost::bind(&LidarLiteDriverNode::ThreadFunc, this))
Thread_(boost::bind(&LidarLiteDriverNode::ThreadFunc, this))
{ {
// this->loop_rate_ = 1000; // [Hz] // this->loop_rate_ = 1000; // [Hz]
// Read from launch file // Read from launch file
std::string serial; std::string serial;
int config_mode; int config_mode;
this->public_node_handle_.param<std::string>("serial_num", serial, "A700evSl"); this->public_node_handle_.param<std::string>("serial_num", serial,
"A700evSl");
this->public_node_handle_.param<int>("config_mode", config_mode, 0); this->public_node_handle_.param<int>("config_mode", config_mode, 0);
this->public_node_handle_.param<std::string>("frame_id", this->frame_id_, "lidar_lite"); this->public_node_handle_.param<std::string>("frame_id", this->frame_id_,
"lidar_lite");
// Set device parameters // Set device parameters
this->driver_.setParams(serial,config_mode); this->driver_.setParams(serial, config_mode);
// [init publishers] // [init publishers]
this->range_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Range>("range", 100); this->range_publisher_ =
this->range_seq_ = 0; this->public_node_handle_.advertise<sensor_msgs::Range>("range", 100);
this->range_seq_ = 0;
// [init subscribers] // [init subscribers]
// [init services] // [init services]
// [init clients] // [init clients]
// [init action servers] // [init action servers]
// [init action clients] // [init action clients]
} }
void LidarLiteDriverNode::ThreadFunc(void) void
LidarLiteDriverNode::ThreadFunc(void)
{ {
while(this->RunThread_) while (this->RunThread_) {
{ if (this->driver_.getState() == LidarLiteDriver::RUNNING) {
if (this->driver_.getState() == LidarLiteDriver::RUNNING)
{
// Get measurement // Get measurement
this->driver_.lock(); this->driver_.lock();
int range = this->driver_.getRange(); int range = this->driver_.getRange();
this->driver_.unlock(); this->driver_.unlock();
// Publish ROS message // Publish ROS message
this->range_msg_.header.seq = this->range_seq_; this->range_msg_.header.seq = this->range_seq_;
this->range_msg_.header.stamp = ros::Time::now(); this->range_msg_.header.stamp = ros::Time::now();
this->range_msg_.header.frame_id = this->frame_id_; this->range_msg_.header.frame_id = this->frame_id_;
this->range_msg_.field_of_view = 0.1; this->range_msg_.field_of_view = 0.1;
this->range_msg_.min_range = 0; this->range_msg_.min_range = 0;
this->range_msg_.max_range = 40; this->range_msg_.max_range = 40;
this->range_msg_.range = range; this->range_msg_.range = range;
this->range_publisher_.publish(this->range_msg_); this->range_publisher_.publish(this->range_msg_);
// Increment range publisher counter. // Increment range publisher counter.
++this->range_seq_; ++this->range_seq_;
} }
} }
} }
void LidarLiteDriverNode::mainNodeThread(void) void
LidarLiteDriverNode::mainNodeThread(void)
{ {
/* code */ /* code */
} }
...@@ -75,27 +77,33 @@ void LidarLiteDriverNode::mainNodeThread(void) ...@@ -75,27 +77,33 @@ void LidarLiteDriverNode::mainNodeThread(void)
/* [action requests] */ /* [action requests] */
void LidarLiteDriverNode::postNodeOpenHook(void) void
LidarLiteDriverNode::postNodeOpenHook(void)
{ {
} }
void LidarLiteDriverNode::addNodeDiagnostics(void) void
LidarLiteDriverNode::addNodeDiagnostics(void)
{ {
} }
void LidarLiteDriverNode::addNodeOpenedTests(void) void
LidarLiteDriverNode::addNodeOpenedTests(void)
{ {
} }
void LidarLiteDriverNode::addNodeStoppedTests(void) void
LidarLiteDriverNode::addNodeStoppedTests(void)
{ {
} }
void LidarLiteDriverNode::addNodeRunningTests(void) void
LidarLiteDriverNode::addNodeRunningTests(void)
{ {
} }
void LidarLiteDriverNode::reconfigureNodeHook(int level) void
LidarLiteDriverNode::reconfigureNodeHook(int level)
{ {
} }
...@@ -107,7 +115,9 @@ LidarLiteDriverNode::~LidarLiteDriverNode(void) ...@@ -107,7 +115,9 @@ LidarLiteDriverNode::~LidarLiteDriverNode(void)
} }
/* main function */ /* main function */
int main(int argc,char *argv[]) int
main(int argc, char* argv[])
{ {
return driver_base::main<LidarLiteDriverNode>(argc, argv, "lidar_lite_driver_node"); return driver_base::main<LidarLiteDriverNode>(argc, argv,
"lidar_lite_driver_node");
} }
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment