Lidar lite V3 ROS Driver

edit

Please try to have a look at this link if you have hard time getting along with Arduino & ROS compatibilities.

Maybe my following code would be quite helpful afterwards. It was tested in Arduino IDE 1.8.5, ROS Kinetic, Ubuntu 16.06.

#include <ros.h>
#include <Wire.h>
#include <LIDARLite.h>
#include <sensor_msgs/Range.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>

#define    LIDARLite_ADDRESS   0x62         
#define    RegisterMeasure     0x00         
#define    MeasureValue        0x04       
#define    RegisterHighLowB    0x8f

#define USB_USBCON

LIDARLite myLidarLite;

ros::NodeHandle  nh;

tf::TransformBroadcaster br;
geometry_msgs::TransformStamped t;

sensor_msgs::Range range_msg;
sensor_msgs::LaserScan scan;

ros::Publisher pub_range("lidarScanner", &scan);

unsigned const int num_readings = 30;
double laser_frequency = 40;
const float pi = 3.14;

float rangeArray[num_readings];
float intensityArray[num_readings];

char base_link[] = "base_link";
char lidar[] = "lidar_lite_v3";

double getRange()
{
 int val = -1;
 Wire.beginTransmission((int)LIDARLite_ADDRESS); 
 Wire.write((int)RegisterMeasure); 
 Wire.write((int)MeasureValue);   
 Wire.endTransmission(); 
 delay(10);

 Wire.beginTransmission((int)LIDARLite_ADDRESS);
 Wire.write((int)RegisterHighLowB); 
 Wire.endTransmission(); 
 delay(10); 
 Wire.requestFrom((int)LIDARLite_ADDRESS, 2);

  if(2 <= Wire.available()) 
  {
   val = Wire.read(); 
   val = val << 8; 
   val = Wire.read();
   }
     return val*.01;
   }

 void setup(){

 Serial.begin(115200);
 nh.initNode();
 br.init(nh);
 nh.advertise(pub_range);

 //delay(1000);
 myLidarLite.begin(0, true);
 myLidarLite.configure(0);

}

void loop(){

  scan.header.frame_id =  lidar;
  scan.header.stamp = nh.now();

  scan.angle_min = 0;
  scan.angle_max = pi;
  scan.angle_increment = pi / num_readings;
  scan.time_increment = (1 / laser_frequency) / (num_readings);
  scan.range_min = 0.0;
  scan.range_max = 40;
  scan.ranges = rangeArray;
  scan.intensities = intensityArray;


  //scan.ranges= ranges;
  for(unsigned int i = 0; i < num_readings; ++i){
      rangeArray[i] = getRange();
      intensityArray[i] = 10 + getRange();
    }

  scan.ranges_length = num_readings;
  scan.intensities_length = num_readings;

  for(unsigned int i = 0; i < num_readings; ++i){
      scan.ranges[i] = rangeArray[i];
      scan.intensities[i] = intensityArray[i];
      }

  pub_range.publish(&scan);


  // tf odom->base_link
  t.header.stamp = nh.now();
  t.header.frame_id = base_link;

  t.child_frame_id = lidar;

  t.transform.translation.x = 0;
  t.transform.translation.y = 0;
  t.transform.translation.z = 1;

  t.transform.rotation = tf::createQuaternionFromYaw(0);

  br.sendTransform(t);

  nh.spinOnce();
  delay(10);
}