LaserScan ROS + Arduino

Introductions:

Yes, a happy weekend plus a holiday on Monday, so with  choco and cookies placed on the table ah what  a little  time to spare   ticking keyboards here. Anyway, we will be continuing our R.O.S tutorials , and its all about understanding how LIDARs works in R.O.S. We knew that the scattered lines served as the perception of a robot using this kind of sensors. These lines forming figures creates a dimensions of area used to be navigated by robots.The rays of line were formed as a collection of distances measured or sensed by LIDAR/LEDDAR).Thus infra red lights emitted by transmitter are then feedback to the receiver usually the photo diode. An array of these measured distances  will be included  in the parameters' value of   scanning angle,time,and the number of points how wide is the coverage of scanning.All these parameters are wrapped in the "Sensor Messages/LaserScan" in a R.O.S string packet format.

This time, the deal is using rosserial library, "ros_lib  and ROS Sensors Messages:: LaserScan. The Arduino DUE will  process the  publishing of  "LaserScan" messages and its topic. Using "ros_lib" an Arduino Due  communicate  with the host R.O.S installed in the computer (Laptop/Desktop) both of which are  connected physically via serial serial interface . In the  near future  of these tutorials, using Arduino/microcomputer will not be ideal anymore in  integrating multiple  sensors to R.O.S. This is because processing and computing will be at risk and not a practical design of  R.O.S application. In short, the use of microcontroller is only to interface sensor modules yet most  of single board computers  have IO ports to their advantage.

Thus , what we will be doing  is to get a common measuring modules such as  SAMSUNG infrared or Ultrasonic SRF04 considered as poor man's LIDAR. SG90 servo motor will carry and rotate the sensor with the speed relative to the frequency of its data acquisition. The angle of rotation is as wide as  180 degrees angle, a back and forth or  a clock and  counter clockwise directions. It is possible to rotate 360 degrees wide  but it requires a slip ring and that is an additional cost. The  interval of time,angle, and number frequency of measuring   distances can be set or adjusted depending the type of sensor or  modules being used. To accurately dump data in  R.O.S Laser scan messages , is to configure its   parameter settings, for example a 180  degrees maximum angle if   divided  by desired 90 points then the angular interval of scanning is 2 degrees.

The simple steps to code ROS Laser Scanning,first  is to initialize  Sensor messages,its laser scan function ,and  to  check topic that publish messages. The finale, use the  R.O.S visualization "rviz" to simulate the functionality of  DIY LIDAR. Okay? So let's get this done :) 

Requirements:
Software: Ubunto 16.04, ROS (kinetic)
Hardware: Arduino DUE, SBC or Laptop,SG90 Servo
                Optional : sensors (ultrasonic or infra red)

Objectives:
1)  To be able to publish a ranging(distance measurement sensor) messages from a DIY LIDAR to ROS .
2) To be able to study algorithm or program to  collect array of measurements and  feed(publish)  it into a R.O.S messages format.
3) To create a program using  rosserial library "ros_lib"  from Arduino to ROS

Methodology:
Testing Arduino Due sample code to fake laser array of data(ranges)

Arduino Due code for Laser scan

Please copy and paste the program
/* Simple code for LaserScan that can be substituted by any ranging sensor (LIDAR,InfraRed,Ultrasound and etc). It give details to the use of sensor message in ROS http://docs.ros.org/melodic/api/sensor_msgs/html/msg/LaserScan.html The code can be used and be ported as is -Thank you. Christopher Coballes ROS Philippines */ #include "math.h" #if defined(ARDUINO) && ARDUINO >= 100 #include "Arduino.h" #else #include "WProgram.h" #endif // ROS includes #define USE_USBCON; #include <ros.h> #include <ros/time.h> #include <sensor_msgs/LaserScan.h> sensor_msgs::LaserScan Laser_msg; #include <Servo.h> // float Angle_min; float Angle_max; float Angle_increment; float Time_increment; float Scan_time; float Range_min; float Range_max; float Ranges[90]; // max of 90 measurements float Intensities[90]; int publisher_timer; int laser_frequency = 100; int point_reading = 90; float time_Now,time_Later; ros:: Publisher pub_Laser("LaserData", &Laser_msg); ros::NodeHandle nh; //char frameid[] ="/sonar_ranger"; Servo Rservo; void setup() { nh.initNode(); nh.advertise(pub_Laser); // Serial.begin(57600); Angle_min = 0; Angle_max = 3.14159265; Angle_increment = 3.14159265 / point_reading; // 90 measurement points Time_increment= ((1 /laser_frequency)/point_reading); // (1 / laser_frequency) / (num_readings); //Scan_time = 1.5; Range_min = 0.0; Range_max = 100.00; Laser_msg.ranges_length = 90; Laser_msg.intensities_length = 90; Rservo.attach(9); initRanges(); } float deg2Pi(float angle) { float rad ; rad = angle /3.14159265 ; return rad; } float getDistance() { float randDistance; randDistance = random(0.5, 4.00); // randDistance = 0.3; return randDistance; } void clearScan() { int tos; // Serial.println("CLEARING"); // delay(300); for (tos = 0; tos <= 180; tos += 2) { // Serial.println(tos+"OK"); Ranges[tos]= 0 ; } } void initRanges() { for (int set_points = 0 ; set_points<90; set_points ++) { Ranges[set_points] = set_points; Intensities[set_points] = set_points*set_points; } } void loop() { if ((millis() - publisher_timer)> 50 ) { Laser_msg.header.stamp = nh.now(); Laser_msg.header.frame_id = "/Lidar_frame"; Laser_msg.angle_min = Angle_min; Laser_msg.angle_max = Angle_max; Laser_msg.angle_increment = Angle_increment; Laser_msg.time_increment = Time_increment; Laser_msg.range_min = Range_min; Laser_msg.range_max = Range_max; // scan_time time_Later = millis(); for (int angle_Setpoints = 0 ; angle_Setpoints < point_reading; angle_Setpoints ++) { delay(5); Rservo.write((angle_Setpoints+1)*2); // Serial.println((angle_Setpoints+1)*2); Ranges[angle_Setpoints]= getDistance(); Laser_msg.ranges[angle_Setpoints] = Ranges[angle_Setpoints]; Laser_msg.intensities[angle_Setpoints] = Intensities[angle_Setpoints]; } time_Now = millis(); Scan_time = time_Now - time_Later; Laser_msg.scan_time = Scan_time; pub_Laser.publish(&Laser_msg); publisher_timer = millis() ; } nh.spinOnce(); // delay(500); }
A simple setup of our DIY Lidar (photo credit to the photographer)

Open four CLI terminals:
 Run the ros core "roscore"
robook# roscore

Communicate between ROS and Arduino  using rosserial
library
robook # rosrun rosserial_python serial_node.py /dev/ttyUSB0
robook# rosrun rosserial_arduino serial_node.py _port:=/dev/ttyUSB0

Make sure that the "/laserScan" topic is being echoed or broadcasted to ROS.
robook# rostopic list
robook# rostopic echo /LaserScan
If  Arduino DUE is able to bombard those collected ranges in ROS , scrolling
stream of string may appear in the screen

Set the transformation of the frame required to accurately map the laser data
robook# rosrun tf tf_publisher_transform 0.0 0.0 0.0 0.0 0.0 0.0  /base_frame /Lidar_frame

Displaying or visualizing  the laser data, we can use RVIZ to plot
it in its grid map.
robook# rosrun rviz rviz 

By setting the ROS Visualizer "rivz": fixed frame "Lidar_frame" and LaserScan
"LaserScan". The display would eventually appear same as the screen below.

Making sure that RVIZ is properly configured , LaserScan
topic is receiving messages coming from Arduino Due,also
transformation is set properly.

Summary:
Trouble:    Arduino is not syncing to rosserial
Shooting:  Place "#define USE_USBCON" before the include "ros.h"
Trouble: No display of laser data in RVIZ
Shooting : Check if there is a laser scan topic
                 Use  #rostopic echo /LaserScan -> if this is the topic

Conclusion:
A simple code of a possible integrations of different ranging sensor to ROS