Anda di halaman 1dari 6

Este documento describe como funciona el laser lidar en Robot operating system (R.O.S.).

El
laser lidar se conecta a la computadora por medio de ethernet, se necesita configurar la
conexion de red de la computadora para que se pueda conectar con la ip del laser cual es
169.254.47.116 , ahora solo se necesita seguir a partir de consola para conectar al dispositivo
por medio del driver creado en R.O.S.
CONSOLA
1) Asegurarse tener de fuente el repositorio correcto de ROS en el cual se encuentra nuestro
paquete del laser.
#>source /opt/ros/DISTRIBUCIONDEROS
2) Ejecutar el nodo del lidar para crear la conexion via ethernet con el dispositivo.
#>rosrun LaserEspol LMS100
3) Verificar la correcta recepcion de datos del laser a traves del siguiente comando:
#>rostopic echo /scan
4) Visualizar los datos del laser a traves de una herramienta grafica en R.O.S. Llamada RVIZ
#>rosrun rviz rviz
Descripcion del nodo y de la libreria del Laser Lidar
//Esta seccion incluimos las librerias que vamos a utilizar para nuestro proyecto.
#include <csignal>
#include <cstdio>
#include "LMS1xx.h" //Esta es la libreria que nos proveera de clases y funciones para el
manejo del laser
#include "ros/ros.h"
#include <LaserEspol/LaserEspol.h>//Esta es la libreria que nos proveera
//de la estructura del mensaje que se va a publicar al topico
#define DEG2RAD M_PI/180.0
int main(int argc, char **argv)
{
// laser data
LMS1xx laserEspol; //Declaramos la variable laserEspol que tiene la clase LMS1xx
scanCfg cfg; //Declaramos la variable cfg que contiene parametros como la frecuencia de
//escaneo, resolucion del angulo, angulo de inicio y angulo final
scanDataCfg dataCfg; //Declaramos la variable datacfg que contiene parametros como el
//canal de salida,remission, resolution, encoder, posicion, nombre del dispositivo y intervalo de
//salida
scanData data;// Declaramos la variable data que contiene toda la informacion de lo que sensa
//nuestro laser, lo que incluye longitud de pulsos, rangos de distancia y remision de cada pulso.
LaserEspol::LaserEspol scan_msg; // Declaramos la variable que contiene las variables de
//nuestro mensaje
std::string host;

std::string frame_id;
ros::init(argc, argv, "lms1xx"); // Se inicia el nodo de ros llama lms1xx
ros::NodeHandle nh; // Declaramos una variable de clase ROS que nos permitira crear un
//publisher para publicar nuestra informacion a traves de los mensajes
ros::NodeHandle n("~");
ros::Publisher scan_pub = nh.advertise<LaserEspol::LaserEspol>("scan", 1); // Se crea una
//variable publisher que incluye nuestro mensaje LaserEspol para poder guardar ahi la
//informacion de nuestro sensor y despues publicarla
ros::Publisher scan_pub1 = nh.advertise<LaserEspol::LaserEspol>("scan1", 1);
ros::Publisher scan_pub2 = nh.advertise<LaserEspol::LaserEspol>("scan2", 1);
ros::Publisher scan_pub3 = nh.advertise<LaserEspol::LaserEspol>("scan3", 1);
ros::Publisher scan_pub4 = nh.advertise<LaserEspol::LaserEspol>("scan4", 1);
n.param<std::string>("host", host, "169.254.47.116"); // Se crea un parametro host para poder
modificar la ip cuando queramos desde nuestro archivo de configuracion de ros .yaml
n.param<std::string>("frame_id", frame_id, "laser");// Se crea un parametro frame_id para
//poder modificar el nombre cuando queramos desde nuestro archivo de configuracion de
ros .yaml
ROS_INFO("connecting to laser at : %s", host.c_str()); // Se imprime en pantalla
laserEspol.connect(host); // La variable laserEspol llama a la funcion connect y le manda
como parametro la ip del host
if (laserEspol.isConnected())
{
ROS_INFO("Connected to laser."); // Se imprime por pantalla
laserEspol.login();// Se loguea para poder acceder a la informacion del sensor
cfg = laserEspol.getScanCfg();// LLama a la funcion get para guardar la informacion de
//algunos parametros en la variable cfg
scan_msg.header.frame_id = frame_id;//Se va definiendo el mensaje comenzando con el
frame_id
scan_msg.range_min = 0.01;/* Se define en el mensaje el rango minimo del laser "este valor
es dado por expermimentos, no se puede setear"*/
scan_msg.range_max = 20.0;/*Se define en el mensaje el rango maximo del laser */
scan_msg.scan_time = 1000.0/cfg.scaningFrequency;//Se define en el mensaje el valor del
//tiempo del escaner, este valor se mide dividiendo 1000 para la frecuencia de escaneo, lo
//retorna en segundos.
scan_msg.angle_increment = cfg.angleResolution/10000.0 * DEG2RAD; /*Se define en el
mensaje este valor del angulo de incremento, este valor se basa en la resolucion del angulo que
es cada 0.5 grados y se lo convierte en radianes */
scan_msg.angle_min = cfg.startAngle/10000.0 * DEG2RAD;/*Se define en el mensaje este
valor del angulo de inicio, este valor se basa en el angulo de inicio que es -45 grados y se lo
convierte en radianes */
scan_msg.angle_max = cfg.stopAngle/10000.0 * DEG2RAD;/*Se define en el mensaje este
valor del angulo final, este valor se basa en el angulo final que es 225 grados y se lo convierte

en radianes */
int num_values; /*En las condiciones se muestra si el angulo de resolucion pueden ser dos, y
si son dos se obtendra tales numeros de
pulsos de nuestro sensor en cada pasada */
if (cfg.angleResolution == 2500)
{
num_values = 1081;
}
else if (cfg.angleResolution == 5000)
{
num_values = 541;
}
else
{
ROS_ERROR("Unsupported resolution");
return 0;
}
scan_msg.time_increment = scan_msg.scan_time/num_values; //Se calcula el tiempo de
incremento del sensor
scan_msg.ranges.resize(num_values);// Se cambia de tamao el arreglo de rangos del mensaje
dependiendo de la resolucion del angulo
scan_msg.intensities.resize(num_values);/*Se cambia de tamao el arreglo de intensidades del
mensaje dependiendo de la resolucion del angulo*/
dataCfg.outputChannel = 1; // Se setea los valores de datacfg para nuestra comodidad de
trabajo
dataCfg.remission = true;
dataCfg.resolution = 1;
dataCfg.encoder = 0;
dataCfg.position = false;
dataCfg.deviceName = false;
dataCfg.outputInterval = 1;
laserEspol.setScanDataCfg(dataCfg); // Llama a la funcion de setsscandatacfg que lleva como
//parametro la variable datacfg para setear los valores de scandatacfg
laserEspol.startMeas(); // Esta funcion le da la seal al laser de que comience a sensar
status_t stat;
do
{
stat = laserEspol.queryStatus();
ros::Duration(1.0).sleep();
}
while (stat != ready_for_measurement);
laserEspol.scanContinous(1); // Esto da la seal al laser de que todo esta bien y siga sensando

while (ros::ok())//Mientras la plataforma ros siga trabajando normalmente el codigo dentro


del while seguira trabajando
{
ros::Time start = ros::Time::now();// Esta variable de tiempo de ros lo que hace es guardar la
hora en que comenzo a trabajar
scan_msg.header.stamp = start;
++scan_msg.header.seq;
laserEspol.getData(data);//Esta funcion retorna con la variable que fue enviada los valores
de lo que sensa el laser
for (int i = 0; i < data.dist_len1; i++) // Se realiza un for donde recorrera el arreglo de los
valores de rangos del laser y llenara nuestro arreglo en nuestro mensaje de rangos
{
scan_msg.ranges[i] = data.dist1[i] * 0.001;
}
for (int i = 0; i < data.rssi_len1; i++)// Este for hace lo mismo que el anterior pero en vez de
rangos son las intensidades
{
scan_msg.intensities[i] = data.rssi1[i];
}
scan_pub.publish(scan_msg); // Se publica nuestro mensaje al topico
for (int i = 0; i < data.dist_len1; i++)
{
if (data.rssi1[i] > 200.0 && data.rssi1[i] < 300.0){
scan_msg.ranges[i] = data.dist1[i] * 0.001;
}
else {
scan_msg.ranges[i] = 0;
}
}
for (int i = 0; i < data.rssi_len1; i++)
{
if (data.rssi1[i] > 200.0 && data.rssi1[i] < 300.0){
scan_msg.intensities[i] = data.rssi1[i];
}
else{
scan_msg.intensities[i] = 0;

}
}
scan_pub1.publish(scan_msg);
for (int i = 0; i < data.dist_len1; i++)
{
if (data.rssi1[i] > 300.0 && data.rssi1[i] < 400.0 ){
scan_msg.ranges[i] = data.dist1[i] * 0.001;
}
else {
scan_msg.ranges[i] = 0;
}
}
for (int i = 0; i < data.rssi_len1; i++)
{
if (data.rssi1[i] > 300.0 && data.rssi1[i] < 400.0){
scan_msg.intensities[i] = data.rssi1[i];
}
else{
scan_msg.intensities[i] = 0;
}
}
scan_pub2.publish(scan_msg);
for (int i = 0; i < data.dist_len1; i++)
{
if (data.rssi1[i] > 410.0 && data.rssi1[i] < 540.0 ){
scan_msg.ranges[i] = data.dist1[i] * 0.001;
}
else {
scan_msg.ranges[i] = 0;
}
}
for (int i = 0; i < data.rssi_len1; i++)
{
if (data.rssi1[i] > 410.0 && data.rssi1[i] < 540.0 ){
scan_msg.intensities[i] = data.rssi1[i];
}
else{
scan_msg.intensities[i] = 0;
}
}
scan_pub3.publish(scan_msg);
for (int i = 0; i < data.dist_len1; i++)
{

if (data.rssi1[i] >500){
scan_msg.ranges[i] = data.dist1[i] * 0.001;
}
else {
scan_msg.ranges[i] = 0;
}
}
for (int i = 0; i < data.rssi_len1; i++)
{
if (data.rssi1[i] > 500 ){
scan_msg.intensities[i] = data.rssi1[i];
}
else{
scan_msg.intensities[i] = 0;
}
}
scan_pub4.publish(scan_msg);
ros::spinOnce();
}
laserEspol.scanContinous(0);// Desactiva el escaneo continuo del laser
laserEspol.stopMeas();//Da la seal de ya no mandar mas pulsos
laserEspol.disconnect();//Desconecta el laser
}
else
{
ROS_ERROR("Connection to device failed");//Si el laser no esta conectado, muestra un error
}
return 0;
}