First node version. Frequencies must be fixed. now running at 40Hz

CMakeLists.txt

@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_driver)
+find_package(catkin REQUIRED COMPONENTS iri_base_driver sensor_msgs)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -14,7 +14,8 @@ find_package(catkin REQUIRED COMPONENTS iri_base_driver)
 # ******************************************************************** 
 #           Add system and labrobotica dependencies here
 # ******************************************************************** 
-# find_package(<dependency> REQUIRED)
+find_package(usb_i2c_adapter REQUIRED)
+find_package(lidar_lite REQUIRED)
 
 # ******************************************************************** 
 #           Add topic, service and action definition here
@@ -49,7 +50,7 @@ find_package(catkin REQUIRED COMPONENTS iri_base_driver)
 # ******************************************************************** 
 #                 Add the dynamic reconfigure file 
 # ******************************************************************** 
-generate_dynamic_reconfigure_options(cfg/LidarLite.cfg)
+generate_dynamic_reconfigure_options(cfg/LidarLiteDriver.cfg)
 
 # ******************************************************************** 
 #                 Add run time dependencies here
@@ -60,11 +61,11 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_driver
+ CATKIN_DEPENDS iri_base_driver sensor_msgs
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
-#  DEPENDS 
+ DEPENDS lidar_lite usb_i2c_adapter
 )
 
 ###########
@@ -76,6 +77,8 @@ catkin_package(
 # ******************************************************************** 
 include_directories(include)
 include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${lidar_lite_INCLUDE_DIR})
+include_directories(${usb_i2c_adapter_INCLUDE_DIR})
 # include_directories(${<dependency>_INCLUDE_DIR})
 
 ## Declare a cpp library
@@ -88,12 +91,15 @@ add_executable(${PROJECT_NAME} src/lidar_lite_driver.cpp src/lidar_lite_driver_n
 #                   Add the libraries
 # ******************************************************************** 
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${lidar_lite_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${usb_i2c_adapter_LIBRARY})
 # target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
 
 # ******************************************************************** 
 #               Add message headers dependencies 
 # ******************************************************************** 
 # add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
 # ******************************************************************** 
 #               Add dynamic reconfigure dependencies 
 # ******************************************************************** 

README.md

@@ -34,6 +34,9 @@ ROS wrapper to work with [LidarLite](https://gitlab.iri.upc.edu/asantamaria/Lida
   - Download the library: `svn co https://devel.iri.upc.edu/pub/labrobotica/ros/iri-ros-pkg_hydro/metapackages/iri_core`
   - Compile: `roscd && cd ../src && catkin_make` 
 
+- [sensor_msgs](http://wiki.ros.org/sensor_msgs) - ROS sensor messages
+
+  - Install: `sudo apt-get install ros-`<ROS VERSION>`-sensor-msgs
 
 ### Example of usage
 

cfg/LidarLite.cfg → cfg/LidarLiteDriver.cfg

@@ -40,5 +40,6 @@ gen = ParameterGenerator()
 
 #       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
 #gen.add("velocity_scale_factor",  double_t,  SensorLevels.RECONFIGURE_STOP,   "Maximum velocity scale factor",  0.5,      0.0,  1.0)
+gen.add("Config_mode", int_t, 0, "LidarLite configuration mode", 0, 1, 5)
 
 exit(gen.generate(PACKAGE, "LidarLiteDriver", "LidarLite"))

include/lidar_lite_driver.h

 // Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
-// Author 
+// Author Angel Santamaria-Navarro 
 // All rights reserved.
 //
 // This file is part of iri-ros-pkg
@@ -28,7 +28,8 @@
 #include <iri_base_driver/iri_base_driver.h>
 #include <iri_lidar_lite/LidarLiteConfig.h>
 
-//include lidar_lite_driver main library
+#include <iridrivers/lidar_lite.h>
+#include <iridrivers/lidar_lite_exceptions.h>
 
 /**
  * \brief IRI ROS Specific Driver Class
@@ -54,6 +55,11 @@ class LidarLiteDriver : public iri_base_driver::IriBaseDriver
   private:
     // private attributes and methods
 
+    CLidarLite* laser_ptr_;
+    bool params_set_;
+    std::string serial_;
+    int config_mode_;
+
   public:
    /**
     * \brief define config type
@@ -145,6 +151,10 @@ class LidarLiteDriver : public iri_base_driver::IriBaseDriver
     // here define all lidar_lite_driver interface methods to retrieve and set
     // the driver parameters
 
+    void setParams(const std::string &serial, const int &config_mode);
+
+    int getRange(void);
+
    /**
     * \brief Destructor
     *

include/lidar_lite_driver_node.h

 // Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
-// Author 
+// Author Angel Santamaria-Navarro
 // All rights reserved.
 //
 // This file is part of iri-ros-pkg
@@ -29,6 +29,7 @@
 #include "lidar_lite_driver.h"
 
 // [publisher subscriber headers]
+#include <sensor_msgs/Range.h>
 
 // [service client headers]
 
@@ -54,7 +55,13 @@
 class LidarLiteDriverNode : public iri_base_driver::IriBaseNodeDriver<LidarLiteDriver>
 {
   private:
+
+    int range_seq_;         // Range publisher counter.
+    std::string frame_id_;  // publisher frame id.
+
     // [publisher attributes]
+    ros::Publisher range_publisher_;
+    sensor_msgs::Range range_msg_;
 
     // [subscriber attributes]
 

launch/iri_lidar_lite.launch

+<!-- <!DOCTYPE html> -->
+
+<launch>
+
+  <node pkg="iri_lidar_lite" type="iri_lidar_lite" name="iri_lidar_lite" output="screen">
+    <param name="serial_num"  type="string" value="A700evSl"/>
+    <param name="config_mode" type="int"    value="1"/>
+  </node>
+
+</launch>

package.xml

@@ -7,7 +7,7 @@
   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="asantamaria@todo.todo">asantamaria</maintainer>
+  <maintainer email="asantamaria@iri.upc.edu">Angel Santamaria-Navarro</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->
@@ -41,7 +41,9 @@
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>iri_base_driver</build_depend>
+  <build_depend>sensor_msgs</build_depend>
   <run_depend>iri_base_driver</run_depend>
+  <run_depend>sensor_msgs</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

src/lidar_lite_driver.cpp

@@ -2,23 +2,47 @@
 
 LidarLiteDriver::LidarLiteDriver(void)
 {
-  //setDriverId(driver string id);
+  this->laser_ptr_ = NULL;
+  this->serial_ = "X000xxXX";
+  this->config_mode_ = 0;
 }
 
 bool LidarLiteDriver::openDriver(void)
 {
-  //setDriverId(driver string id);
-
   return true;
 }
 
 bool LidarLiteDriver::closeDriver(void)
 {
+  try
+  {
+    this->laser_ptr_->close();
+  }catch(CLidarLiteException &e)
+  {
+    std::cout << e.what() << std::endl;
+  }
+
+  if(this->laser_ptr_ != NULL)
+  {
+    this->laser_ptr_ = NULL;
+    delete this->laser_ptr_;
+  }  
+
   return true;
 }
 
 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;
 }
 
@@ -42,6 +66,18 @@ void LidarLiteDriver::config_update(Config& new_cfg, uint32_t level)
       break;
 
     case LidarLiteDriver::RUNNING:
+
+      this->config_mode_ = new_cfg.Config_mode;
+
+      try
+      {
+        this->laser_ptr_->set_new_config(this->config_mode_);
+      }
+      catch(CLidarLiteException &e)
+      {
+        std::cout << e.what() << std::endl;
+      }
+
       break;
   }
 
@@ -51,6 +87,33 @@ void LidarLiteDriver::config_update(Config& new_cfg, uint32_t level)
   this->unlock();
 }
 
+void LidarLiteDriver::setParams(const std::string &serial, const int &config_mode)
+{
+  this->serial_ = serial;
+  this->config_mode_ = config_mode;
+}
+
+int LidarLiteDriver::getRange(void)
+{
+  int range;
+
+  try
+  {
+    range = this->laser_ptr_->get_range(true);
+  }
+  catch(CLidarLiteException &e)
+  {
+    std::cout << e.what() << std::endl;
+  }
+
+  return range;
+}
+
 LidarLiteDriver::~LidarLiteDriver(void)
 {
+  if(this->laser_ptr_ != NULL)
+  {
+    this->laser_ptr_ = NULL;
+    delete this->laser_ptr_;
+  }  
 }

src/lidar_lite_driver_node.cpp

@@ -3,11 +3,38 @@
 LidarLiteDriverNode::LidarLiteDriverNode(ros::NodeHandle &nh) : 
   iri_base_driver::IriBaseNodeDriver<LidarLiteDriver>(nh)
 {
-  //init class attributes if necessary
-  //this->loop_rate_ = 2;//in [Hz]
+  // Read from launch file
+  std::string serial;
+  this->public_node_handle_.param<std::string>("serial_num", serial, "A700evSl");
 
-  // [init publishers]
+  int config_mode;
+  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_seq_ = 0;  
+
   // [init subscribers]
   
   // [init services]
@@ -24,15 +51,29 @@ void LidarLiteDriverNode::mainNodeThread(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_;
 
   //unlock access to driver if previously blocked
   this->driver_.unlock();