Lab 2.2: Use Laser Range Finder-Hokuyo

Overview

The goal of this lab is to initial the Hokuyo laser module, subscribe and modify the data from it.

First make sure that Hokuyo node is installed :

sudo apt-get install ros-indigo-hokuyo-node

Power on the Hokuyo in wifibot

By default, the Hokuyo module is not powered, we have to send command to DSP board to activate the power for Hokuyo every time we restart the wifibot. The command has to be sent via serial port to DSP board, therefore, writing a c++ script to send the command will be convenient every time we restart the wifibot.

Open terminal and use gedit to write a script:

gedit HokuyoIni.cpp

Use following code:

#include <stdio.h>
#include <termios.h> //header contains the definitions used by the terminal I/O interfaces
#include <unistd.h> //read() write() close()
#include <fcntl.h>
#include <string.h>
#include <stdlib.h>
#include <iostream>
#include <sys/stat.h>
#include <stdint.h>
#define BAUDRATE B19200
#define SERIAL_PATH “/dev/ttyS0”
using namespace std;
int serial_fd;
int serial_read_ret, serial_write_ret;
struct termios serial_settings;
char serial_buffer_send[9];
char serial_buffer_recv[22] = {0};
int Arr[7];
short Crc16(char *Adresse_tab , unsigned char Taille_max)
{
unsigned int Crc = 0xFFFF;
unsigned int Polynome = 0xA001;
unsigned int CptOctet = 0;
unsigned int CptBit = 0;
unsigned int Parity= 0;
Crc = 0xFFFF;
Polynome = 0xA001;
for ( CptOctet= 0 ; CptOctet < Taille_max ; CptOctet++)
{
Crc ^= *( Adresse_tab + CptOctet);
for ( CptBit = 0; CptBit <= 7 ; CptBit++)
{
Parity= Crc;
Crc >>= 1;
if (Parity%2 == true) Crc ^= Polynome;
}
}
return(Crc);
}
int main()
{
serial_buffer_send[0]=(char)255;
serial_buffer_send[1]=(char)0X07;
serial_buffer_send[2]=(char)0X00;
serial_buffer_send[3]=(char)0X00;
serial_buffer_send[4]=(char)0X00;
serial_buffer_send[5]=(char)0X00;
serial_buffer_send[6]=(char)0X51;
short mycrcsend = Crc16(serial_buffer_send+1,6);
serial_buffer_send[7] = (char)mycrcsend;
serial_buffer_send[8] = (char)(mycrcsend >> 8);
printf(“Opening %s in Read/Write mode at 19200 8-N-1…”,SERIAL_PATH);
fflush(stdout);
//Try opening serial port
serial_fd = open(SERIAL_PATH,O_RDWR|O_NOCTTY);
if(serial_fd == -1) { //Checks the availability of the Serial Port
printf(“Failed.\n”);
fflush(stdout);
return 0;
} else {
printf(“Success.\n”);
fflush(stdout);
tcgetattr(serial_fd, &serial_settings); //Get Current Settings of the Port
cfsetispeed(&serial_settings,BAUDRATE); //Set Input Baudrate
cfsetospeed(&serial_settings,BAUDRATE); //Set Output Baudrate
serial_settings.c_cflag &= ~PARENB; //Mask Parity Bit as No Parity
serial_settings.c_cflag &= ~CSTOPB; //Set Stop Bits as 1 or else it will be 2
serial_settings.c_cflag &= ~CSIZE; //Clear the current no. of data bit setting
serial_settings.c_cflag |= CS8; //Set no. of data bits as 8 Bits
}
serial_write_ret = write(serial_fd,serial_buffer_send,sizeof(serial_buffer_send));
return 0;
}

Compile it using g++:

g++ HokuyoIni.cpp -o HokuyoIni

 

Then you will see HokuyoIni has been compiled. Use it by typing in terminal:

./HokuyoIni

Then you will heard a tiny sound from Hokuyo module as the laser begin rotating . Once the Hokuyo module is activate, it will be connected as ttyACM port. You can check the devices that use ttyACM serial port by typing:

dmesg | grep tty

If you see following message, it means you have more than one ttyACM serial port:

[    0.000000] console [tty0] enabled
[    0.971106] 00:08: ttyS0 at I/O 0x3f8 (irq = 4, base_baud = 115200) is a 16550A
[    0.991532] 00:09: ttyS1 at I/O 0x2f8 (irq = 3, base_baud = 115200) is a 16550A
[    3.900274] cdc_acm 3-10:1.0: ttyACM0: USB ACM device
[ 1953.385803] cdc_acm 3-9:1.0: ttyACM1: USB ACM device

Make sure what port Hokuyo module uses, basically it is connected to ttyACM1. Since the IMU module is assigned with ttyACM0 when the wifibot start, and unless you power up the Hokuyo, the ttyACM1 port will not appear.

check the granted permissions on ttyACM1

   ls -l /dev/ttyACM1

It should be like this

   crw-rw-rw- 1 root dialout 166, 0 sept. 14 15:57 /dev/ttyACM1

If there was no full permission, make the module be full controlled with selected ttyACM port, following is for ACMtty1:

sudo chmod a+rw /dev/ttyACM1

Check whether the module is connect using following command:

rosrun hokuyo_node getID /dev/ttyACM1

If the module connect properly, then you will see something like:

Device at /dev/ttyACM1 has ID H1405053

If you do not receive any respond, please close terminal and run getID command again with another ttyACM port.

Run the Hokuyo node in ROS:

rosrun hokuyo_node hokuyo_node _port:=”/dev/ttyACM1″

If everything goes well, you will see:

[INFO] [1438918531.400035799]: Connected to device with ID: H1405053
[INFO] …

And now the Hokuyo module is working and the laser data has been published. Check the data using:

rostopic list

You will see all topics published from all nodes:

/diagnostics
/hokuyo_node/parameter_descriptions
/scan

The /scan data is the laser range data we want, we can now write the script to subscribe the data and use it for your purposes. We can see what composes /scan by typing:

rostopic echo /scan

We can also visualize the /scan data in the rviz.

1. Open revise and then add LaserScan display.

2. Select the topic of LaserScan to /scan.

3. In Fixed Frame field, type laser.

Then you can now see the laser scan data as following image.

rvizforHokuyo